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ABSTRACT 

There  are  many  diverse  numerical  methods  that  can  be  applied  to  solving  path 
planning  problems,  however  most  of  these  are  either  not  valid  or  impractical 
for  solving  anisotropic  (direction-dependent)  path  planning  problems.  Ordered 
Upwind  Methods  (OUM)  are  a  family  of  numerical  methods  for  approximating 
the  viscosity  solution  of  static  Hamilton- Jacobi- Bellman  equations,  and  have 
been  tailored  to  solve  anisotropic  optimal  control  problems. 

There  is  little  information  in  the  literature  regarding  the  implementation  of 
OUM,  and  a  wide  range  of  computational  techniques  and  meticulous  algorith¬ 
mic  considerations  are  required  to  successfully  implement  OUM.  A  comprehen¬ 
sive,  generic  implementation  of  OUM  is  documented  in  this  report,  with  the 
intention  of  minimising  the  technical  barriers  to  employing  OUM  in  real-world 
applications. 
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Globally  Optimal  Path  Planning  with  Anisotropic  Running 

Costs 

Executive  Summary 

Path  planning  is  the  task  of  selecting  a  path  through  an  environment  for  an  entity  to 
traverse  such  that  a  cost  is  minimised.  Path  planning  has  numerous  applications,  for 
example,  in  robotics,  computer  game  development,  and  controlling  unmanned  vehicles. 
This  report  emerged  from  a  study  of  path  planning  for  military  aircraft  conducting  missions 
in  hostile  environments. 

In  this  report,  an  entity  is  considered  to  follow  a  path  in  Euclidean  space  and  incur  a 
strictly  positive  running  cost  at  each  instant  in  time.  The  (total)  cost  is  the  integral  of 
the  running  cost  over  the  path,  and  the  aim  is  to  steer  the  entity  through  its  environment 
such  that  this  cost  is  minimised. 

The  running  cost  is  considered  to  be  anisotropic,  that  is,  it  depends  on  the  position 
and  velocity  vector  of  the  entity.  Hence  for  anisotropic  path  planning  problems,  the  cost 
depends  on  the  location  of  the  entity  in  its  environment  and  how  it  arrived  at  that  location. 
In  applications,  anisotropic  running  costs  typically  stem  from  heterogeneous  environments, 
and  cases  where  the  orientation  of  the  entity  has  a  significant  impact  on  the  cost. 

There  are  many  diverse  numerical  methods  that  can  be  applied  to  solving  path  planning 
problems,  however  most  of  these  are  either  not  valid  or  impractical  for  solving  anisotropic 
path  planning  problems.  Ordered  Upwind  Methods  (OUM)  are  a  family  of  numerical 
methods  for  approximating  the  viscosity  solution  of  static  Hamilton- Jacobi-Bellman  equa¬ 
tions,  and  have  been  tailored  to  solve  anisotropic  optimal  control  problems.  The  focus  of 
this  report  is  on  the  control-theoretic  OUM. 

There  is  little  information  in  the  literature  regarding  the  implementation  of  OUM, 
and  a  wide  range  of  computational  techniques  and  meticulous  algorithmic  considerations 
are  required  to  successfully  implement  OUM.  A  comprehensive,  generic  implementation  of 
OUM  is  documented  in  this  report,  with  the  intention  of  minimising  the  technical  barriers 
to  employing  OUM  in  real-world  applications. 

The  control-theoretic  OUM  has  been  employed  to  solve  a  number  of  simple  path  plan¬ 
ning  problems  in  this  report,  to  provide  figures  with  which  the  reader  can  compare  output 
from  their  implementation  of  the  OUM. 

The  application  of  OUM  to  the  path  planning  of  military  aircraft  traversing  hostile 
environments  is  the  subject  of  a  future  report. 


iii 


UNCLASSIFIED 


DSTO-TR-2815 


UNCLASSIFIED 


THIS  PAGE  IS  INTENTIONALLY  BLANK 


IV 


UNCLASSIFIED 


UNCLASSIFIED 


DSTO-TR-2815 


Author 


Jason  Looker 

Air  Operations  Division 

Dr  Jason  Looker  joined  DSTO  in  2006  as  an  Operations  Re¬ 
search  Scientist  after  completing  a  BSc  (Honours)  and  PhD  in 
Mathematics  at  The  University  of  Melbourne.  He  has  under¬ 
taken  operations  analysis  in  support  of  Air  Force  Headquarters 
and  AIR  9000  Phase  8  (Future  Naval  Aviation  Combat  Sys¬ 
tem),  and  is  leading  a  research  project  on  the  path  planning  of 
military  aircraft  through  hostile  environments. 


UNCLASSIFIED 


v 


DSTO-TR-2815 


UNCLASSIFIED 


THIS  PAGE  IS  INTENTIONALLY  BLANK 


vi 


UNCLASSIFIED 


UNCLASSIFIED 


DSTO-TR-2815 


Contents 


Glossary  xi 

Notation  xii 

1  Introduction  1 

1.1  Optimal  Control .  2 

1.2  Admissible  Controls .  3 

1.3  Parametrisation  .  4 

1.4  Isotropic  Running  Costs .  4 

2  Ordered  Upwind  Method  5 

2.1  Computational  Mesh .  5 

2.2  Upwinding  Approximation  to  the  DPP .  5 

2.3  OUM  Sets .  6 

2.4  The  OUM  Algorithm  .  9 

3  Implementation  10 

3.1  Mesh  Functions .  10 

3.1.1  Mesh  Transformations .  11 

3.1.2  Mesh  Construction .  12 

3.1.3  Neighbourhood  Functions  .  12 

3.2  Set  Construction .  13 

3.2.1  Adjacency  Function .  13 

3.2.2  AcceptedFront  Construction .  14 

3.2.3  AF  Construction .  14 

3.2.4  NF(z,j)  Construction .  15 

3.3  Tentative  Value  Function .  18 

3.3.1  Local  Minimisation .  18 

3.3.2  Evaluation .  19 

3.4  The  OUM  Algorithm  Revisited .  21 

3.5  Visualisation .  23 

3.5.1  Path  Reconstruction .  23 

3.5.2  Level  Sets .  27 


vii 


UNCLASSIFIED 


DSTO-TR-2815 


UNCLASSIFIED 


4  Examples  27 

4.1  Isotropic  Running  Costs .  28 

4.2  Anisotropic  Running  Costs .  30 

5  Conclusion  33 

References  34 

Appendices 

A  Triangulated  Mesh  Based  on  a  Cartesian  Grid  39 

B  Verification  of  the  AcceptedFront  update  rule  41 


viii 


UNCLASSIFIED 


UNCLASSIFIED 


DSTO-TR-2815 


Figures 

1  An  illustration  of  the  Accepted  (A),  AcceptedFront  (A),  Considered  (C)  and 

Far  (dots)  sets.  The  line  segments  of  the  AF  set  are  indicated  by  lines  with 
arrowheads .  6 

2  Definition  diagram  for  illuminating  the  origins  of  the  NF(x)  set .  7 

3  The  N(i,j )  set  shown  mapped  onto  flh  as  large  dots,  where  the  x(  • ,  • )  are 

given  by  Equation  (17) .  13 

4  Definition  diagram  for  the  FindSimplex  function  (Algorithm  9),  showing  the 

neighbours  of  xc  =  (xc,  yc)  G  and  an  example  where  the  simplex  xcX3X4 
contains  xell .  24 

5  Uniform  running  cost  (R  =  1),  showing  level  sets  and  an  optimal  path  with 

initial  position  x  =  (0.1,  0.1)  and  target  location  X7-  =  (1,1) .  28 

6  Isotropic  running  cost  (given  by  Equation  (30)),  showing  level  sets  and  an 
optimal  path  with  initial  position  x  =  (0.1, 0.1)  and  target  location  x-j  = 

(1, 1).  The  black  squares  represent  obstacles,  which  have  a  side  length  of  0.1 

and  are  located  at  (0.25,0.5),  (0.5,  0.3)  and  (0.65,0.75) .  29 

7  Isotropic  running  cost  (given  by  Equation  (30)),  showing  level  sets  and  a 
representation  of  a  subset  of  the  optimal  controls.  The  target  location  is 

X7-  =  (1, 1)  and  the  {0.1, 0.2, . . . ,  1.2}  level  sets  are  displayed .  29 

8  Anisotropic  running  cost  (given  by  Equation  (31)),  showing  level  sets  and  an 
optimal  path  with  initial  position  x  =  (0.1, 0.1)  and  target  location  x-j  = 

(0.5,  0.5) .  30 

9  Anisotropic  running  cost  (given  by  Equation  (31)),  showing  level  sets  gener¬ 
ated  by  a  “contour”  function  (thick  dashed  curves)  and  the  level  sets  gener¬ 
ated  using  Equation  (29)  (thin  solid  curves).  The  {0.2,  0.4, . . . ,  2.0}  level  sets 

are  displayed .  31 

10  Anisotropic  running  cost  (given  by  Equation  (32)),  showing  level  sets  and 
an  optimal  path  with  initial  position  x  =  (—0.3,  —0.4)  and  target  location 

x^  =  (0,  0).  The  {0.05, 0.0973684, . . . ,  0.95}  level  sets  are  displayed .  32 

11  Anisotropic  running  cost  (given  by  Equation  (32)),  showing  level  sets  and 
a  representation  of  a  subset  of  the  optimal  controls.  The  target  location  is 

x-j  =  (0,  0)  and  the  {0.05,  0.0973684, . . . ,  0.95}  level  sets  are  displayed . 32 

12  Anisotropic  running  cost  (given  by  Equation  (32)),  showing  level  sets  gener¬ 

ated  by  OrderedUpwindMethod  (dashed  curves)  and  the  level  sets  reproduced 
from  Figure  5  of  Sethian  &  Vladimirsky  [2003]  (solid  curves);  both  sets  of 
contours  were  generated  on  a  3852  Cartesian  grid . 33 

A1  The  N(i,j)  set  shown  mapped  onto  as  large  dots  for  the  case  of  a  trian¬ 
gulation  based  on  a  Cartesian  grid.  The  x(  • ,  • )  are  given  by  Equation  (Al) 
and  the  triangulation  diameter  h  =  \/26,  where  5  is  the  grid  spacing .  39 


UNCLASSIFIED 


IX 


DSTO-TR-2815 


UNCLASSIFIED 


THIS  PAGE  IS  INTENTIONALLY  BLANK 


x 


UNCLASSIFIED 


UNCLASSIFIED 


DSTO-TR-2815 


DPP 

HJB 

OUM 


Glossary 


Dynamic  Programming  Principle 
Hamilton- Jacobi-Bellnran 
Ordered  Upwind  Method(s) 


UNCLASSIFIED 


xi 


DSTO-TR-2815 


UNCLASSIFIED 


Notation 

Vectors  in  real  n-dimensional  space  are  denoted  by  bold  symbols,  with  the  exception  of 
the  control  a,  which  can  be  a  scalar  or  an  m-dimensional  vector. 

Mn  real  n-dimensional  space 

x  a  generic  n-dimensional  vector,  x  =  (xi,x2,  ■  ■  ■ ,  xn) 

||x||  magnitude  of  x,  ||x||  =  \JYH=  i  xi 

1 1 x 1 1 oo  infinity-norm,  Hx^  =  max {|aq|,  \x2\,  ■  ■  ■ ,  \xn\} 

x  •  y  scalar  (dot)  product,  x  •  y  =  Ya= i  x^i 

0  empty  set 

fi  set  intersection 

U  set  union 

\  set  difference  (relative  complement) 

C  subset 

C  subset  or  equal 

x  Cartesian  product 

D  an  open  domain,  D  C  Mn 

t  time 

s  arc  length  as  a  parameter 

yx  path  with  initial  position  at  x 

yx  derivative  of  the  path  (velocity) 

f  dynamics  of  the  path 

/  speed  of  the  path 

a  control 

a*  (approximate)  optimal  control 

J  cost  functional 

tx(ct)  exit  time  associated  with  a  and  x  (earliest  time  for  y x(t,  a)  to  hit  the  target  set) 

lx(at)  arc  length  associated  with  a  and  x  (shortest  arc  length  for  y x(s,  ct)  to  hit  the  target  set) 

R  running  cost  per  unit  time 

R  running  cost  per  unit  length 

g  terminal  cost 

T  closed  target  set  where  Tcfi 

u  value  function 

inf  infimum  (greatest  lower  bound) 
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A 

e* 

V 
h 

Xh 

Xj 

% 

Q3 

^XjXfc(x) 

V 
u 

NF(x) 

NF(i,j) 

T(x) 

6 

Z 

(lj) 
x(i,  j) 
nf 

n-TL 

Jh 

int(S') 

XT 

ef 

Ph{fy 
N(i,j ) 
N(5) 
iv(Lj) 

A(p) 

argmin 
UyZ(x,  C) 

T(i,j) 


set  of  admissible  controls 
ith  standard  basis  vector  in  Mn 

gradient  vector  differential  operator,  V  =  Ya= i  e*  WF 

triangulation  diameter 

triangulated  nresh  of  diameter  h 

a  mesh  point  in 

discretisation  of  D 

discretisation  of  T 

nhxnhx  nh 

tentative  value  function  in  the  simplex  XjXXj, 

tentative  value  function 

numerical  value  function 

near  front  set  as  a  function  of  x  6  fl/, 

near  front  set  as  a  function  of  (i,j)  £  Uf 

anisotropy  function  at  x 

Cartesian  grid  spacing 

set  of  integers 

integer  mesh  co-ordinate 

mesh  point  in  f with  integer  mesh  co-ordinate  (i,  j) 
set  of  integer  mesh  co-ordinates 

set  of  integer  mesh  co-ordinates  corresponding  to  7), 

interior  of  a  set  S 

target  location  in  int(T) 

ith  triangulation  unit  vector 

computational  radius 

neighbours  of  the  mesh  co-ordinate  (i,  j) 

neighbours  of  the  set  of  mesh  co-ordinates  S,  N(S )  C  \  S 

neighbourhood  of  the  mesh  co-ordinate  N(i,j)  =  N(i,j)  U  {(i,j)} 

set  of  all  adjacent  pairs  in  N(i,j) 

discriminant  of  the  polynomial  p,  see  Equation  (26) 

argument  of  the  minimum  of  a  function 

see  Equation  (28) 

the  triplet  {(i,j),V(i,j)  ,a(i,j)}  for  (i.  j)  e  Considered 
the  triplet  {(i,j),U(i,j)  ,cx*(i,j)}  for  (i,j)  G  Accepted 
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1  Introduction 

Path  planning  is  the  task  of  selecting  a  path  through  an  environment  for  an  entity  to 
traverse  such  that  a  cost  is  minimised.  Path  planning  has  numerous  applications,  for 
example,  in  robotics,  computer  game  development,  and  controlling  unmanned  vehicles. 
This  report  emerged  from  a  study  of  path  planning  for  military  aircraft  conducting  missions 
in  hostile  environments. 

In  this  report,  an  entity  is  considered  to  follow  a  path  in  Euclidean  space  and  incur  a 
strictly  positive  running  cost  at  each  instant  in  time.  The  (total)  cost  is  the  integral  of 
the  running  cost  over  the  path,  and  the  aim  is  to  steer  the  entity  through  its  environment 
such  that  this  cost  is  minimised. 

If  there  exists  a  path  through  the  environment  that  incurs  a  lower  cost  than  all  other 
paths,  then  this  path  is  globally  optimal.  Whereas  if  there  exists  a  path  through  the 
environment  that  incurs  a  lower  cost  than  all  neighbouring  paths  (in  some  sense),  then 
this  path  is  locally  optimal.  A  globally  optimal  path  is  also  locally  optimal,  however  a 
locally  optimal  path  may  not  be  globally  optimal.  Indeed,  it  is  possible  that  a  locally 
optimal  path  may  represent  a  poor  choice. 

If  the  running  cost  only  depends  on  the  position  of  the  entity,  then  the  running  cost 
is  isotropic.  Whereas  the  running  cost  is  anisotropic  if  it  also  depends  on  the  velocity 
vector  of  the  entity.  Hence  for  anisotropic  path  planning  problems,  the  cost  depends 
on  the  location  of  the  entity  in  its  environment  and  how  it  arrived  at  that  location.  In 
applications,  anisotropic  running  costs  typically  stem  from  heterogeneous  environments, 
and  cases  where  the  orientation  of  the  entity  has  a  significant  impact  on  the  cost. 

This  report  details  a  comprehensive,  generic  implementation  of  a  numerical  method 
for  approximating  all  globally  optimal  paths  for  anisotropic  path  planning  problems.  The 
application  of  this  numerical  method  to  the  path  planning  of  military  aircraft  traversing 
hostile  environments  is  the  subject  of  a  future  report. 

Obstacle  avoidance  is  a  category  of  path  planning  where  the  environment  consists  of 
two  distinct  regions:  one  where  the  entity  is  free  to  move  at  a  finite  cost,  and  another 
where  the  entity  is  forbidden  to  enter.  The  literature  on  obstacle  avoidance  is  vast,  par¬ 
ticularly  in  the  fields  of  robotics  and  artificial  intelligence,  and  methods  to  solve  obstacle 
avoidance  problems  have  been  developed  to  exploit  the  allowed/forbidden  nature  of  the 
environment  to  enable  real-time  computations.  Since  the  path  planning  problem  that  mo¬ 
tivated  this  report  is  not  of  an  obstacle-avoidance  type,  the  obstacle  avoidance  literature 
is  not  discussed  here.  For  an  introduction  to  obstacle  avoidance,  refer  to  the  survey  article 
by  Hwang  &  Ahuja  [1992]  and  the  paper  by  LaValle  &  Kuffner,  Jr.  [2001].  Note  that  the 
numerical  method  that  is  the  subject  of  this  report  can  be  used  for  obstacle  avoidance 
problems. 

There  are  many  diverse  approaches  for  solving  path  planning  problems.  These  include 
methods  from  optimal  control  that  are  based  on  Pontryagin’s  Minimum  Principle  [Vian  & 
Moore  1989],  nonlinear  programming  [Betts  1998,  Kabamba,  Meerkov  &  Zeitz  III  2006], 
and  viscosity  solutions  of  Hamilton- Jacob i-Bellman  equations  [Tsitsiklis  1995,  Mitchell  & 
Sastry  2003,  Petres  et  al.  2007];  the  calculus  of  variations  [Pachter  &  Hebert  2001,  Novy, 
Jacques  &  Pachter  2002,  Sidhu  et  al.  2006,  Zabarankin,  Uryasev  &  Murphey  2006];  rnath- 
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ematical  programming  [Kim  &  Hespanha  2003,  Chaudhry,  Misovec  &  D’Andrea  2004, 
Zabarankin,  Uryasev  &  Murphey  2006,  Muhandiramge,  Boland  &  Wang  2009];  and  heuris¬ 
tics  based  on  Voronoi  graphs  and  nonlinear  programming  [Dai  &  Cochran  Jr.  2010],  non¬ 
linear  trajectory  generation  [Milam,  Mushambi  &  Murray  2000,  Inane  et  al.  2008],  a 
system  of  virtual  springs  and  masses  [Bortoff  2000,  Mercer  &  Sidhu  2007,  Rowe,  Sidhu  & 
Mercer  2009],  using  simulation  [Beard  et  al.  2002,  McLain  &  Beard  2005],  and  evolution¬ 
ary  computation  [Zheng  et  al.  2005] .  Unfortunately,  all  of  these  approaches  suffer  from  at 
least  one  of  the  following  limitations: 


•  only  locally  optimal  solutions  are  calculated; 

•  are  heuristic  and  hence  may  not  be  locally  optimal; 

•  are  not  valid  for  anisotropic  running  costs;  or 

•  may  not  converge  to  the  optimal  path  as  the  computational  mesh  is  refined. 


The  final  point  primarily  arises  in  graph-based  methods,  and  has  profound  implications  for 
the  physical  interpretation  of  the  solution.  For  a  discussion  of  this  limitation,  refer  to  the 
works  of  Sethian  [1999a],  Sethian  [19996],  and  Muhandiramge,  Boland  &  Wang  [2009].  A 
technique  that  does  not  suffer  from  any  of  these  limitations  is  the  control-theoretic  Ordered 
Upwind  Method  [Sethian  &  Vladimirsky  2001,  Vladimirsky  2001,  Sethian  &  Vladimirsky 
2003,  Alton  &  Mitchell  2012], 

Ordered  Upwind  Methods  (OUM)  are  a  family  of  numerical  methods  for  approximating 
the  viscosity  solution  of  static  Hamilton- Jacobi- Bellman  equations.  OUM  closely  resem¬ 
ble  Dijkstra’s  seminal  shortest-path  algorithm  for  graphs  [Dijkstra  1959],  and  bear  scant 
resemblance  to  the  usual  finite  difference  methods  for  partial  differential  equations.  The 
focus  of  this  report  is  on  control-theoretic  OUM,  which  are  based  on  a  first  order  upwind 
approximation  to  the  Dynamic  Programming  Principle  of  optimal  control.1 

The  aim  of  this  report  is  to  document  a  generic  implementation  of  the  control-theoretic 
OUM,2  as  little  detailed  information  is  available  in  the  literature.  The  OUM  is  introduced 
in  Section  2,  our  implementation  can  be  found  in  Section  3,  and  simple  path  planning 
examples  have  been  included  in  Section  4  to  provide  the  reader  with  points  of  comparison. 


1.1  Optimal  Control 


The  aspects  of  optimal  control  that  are  most  relevant  to  OUM  in  a  path  planning  context 
are  outlined  in  this  section.  Refer  to  Bardi  &  Capuzzo-Dolcetta  [2008,  particularly  Chapter 
IV]  for  a  comprehensive  exposition  on  optimal  control  and  viscosity  solutions  of  Hamilton- 
Jacobi- Bellman  equations;  Evans  [1998]  is  also  an  excellent  resource. 

1  Other  OUM  are  based  on  upwind  finite  difference  discretisations  of  the  Hamilton- Jacobi-Bellman 
equation,  however  a  proof  of  convergence  for  this  approach  for  anisotropic  control  problems  does  not  exist 
to  the  author’s  knowledge  [Sethian  &  Vladimirsky  2003]. 

2  “Control-theoretic  OUM”  and  “OUM”  will  be  used  synonymously  for  the  remainder  of  this  report. 
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Let  fl  C  Mn  be  an  open  domain.  Initially  the  entity  is  at  a  position  x  e  H,  then  for 
each  instant  in  time  t  >  0  the  entity  follows  the  path  yx(t)  according  to  the  state  equation 

y x(i)  =  f(yx(i),  a(*))  (i  >  0), 
yx(0)  =  x, 

where  yx  is  the  velocity,  f  is  the  (Lipschitz-continuous)  dynamics,  and  a  denotes  the 
control  that  steers  the  entity  through  its  environment  (see  Section  1.2). 

The  cost  is  defined  for  each  x  and  ct  to  be 

/•M°0  _ 

J(x,  a)  =  /  R(yx(t),a(t))dt  + g(yx(tx(a))). 

Jo 

Here  tx  ( a )  is  the  exit  time  associated  with  a  and  x,  which  is  the  earliest  time  for  y x(t,  a) 
to  arrive  at  the  closed  target  set  T  C  D,  R  is  the  running  cost  per  unit  time,  and  g  is  the 
terminal  cost.3  Note  that  tx(a)  needs  to  be  determined:  it  is  not  an  input. 

The  goal  of  path  planning  is  to  select  a  control  from  an  admissible  set  A  such  that 
the  cost  is  minimised.  The  value  function  u(x)  is  defined  to  be  the  cost  associated  with  a 
globally  optimal  path  with  initial  position  x: 

u(x)  =  inf  J{x,  a). 
aeA 

The  value  function  satisfies  a  functional  equation  referred  to  as  the  Dynamic  Programming 
Principle  (DPP): 

tt(x)  =  jinf^  jy  i?(yx(i),a(t))di  +  u(yx(r))|  (0  <  r  ^  ix(a)). 

If  there  exists  a  differentiable  solution  «(x)  of  the  DPP,  then  it  can  be  shown  that  u(x) 
also  satisfies  a  nonlinear  first  order  partial  differential  equation  known  as  the  (static) 
Hamilton- Jacobi-Bellman  (HJB)  equation: 

/  mi5  {f(x,  a)  ■  V«(x)  +  R(x,  ct)}  =  0  in  D  \  T, 

yu(x)  =  g(x)  on  T. 

However,  a  differentiable  solution  of  the  DPP  may  not  exist,  and  solutions  of  the  HJB 
equation  are  known  to  be  non- unique  and  possess  shock  discontinuities. 

A  viscosity  solution  is  a  type  of  weak  solution  that  has  been  developed  to  address  these 
issues.  In  particular,  viscosity  solutions  of  the  HJB  equation  are  unique,  continuous,  equal 
to  the  differentiable  solution  at  points  of  differentiability,  and  equal  to  the  value  function. 


1.2  Admissible  Controls 

The  control  a  is  considered  to  be  a  unit  vector  for  the  remainder  of  this  report.  In  a 
path  planning  context,  a  is  typically  the  unit  velocity  of  the  entity  and  the  dynamics  are 
expressed  as 

f(y x(t),a(t))  =  /(yx(t),  a(t))  a(t ) , 

3It  is  assumed  that  R  is  strictly  positive,  g  is  positive,  and  are  both  Lipschitz-continuous  functions. 
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where  /  >  0  is  the  speed  of  the  entity  and  || oe ||  =  1.  The  set  of  admissible  controls  is  then 
A  =  {a  :  M+  U  {0}  e- >  Si  |  a  is  measurable}  , 
where  M+  is  the  set  of  positive  real  numbers,  and  S\  =  {a  G  Mn  |  || oe ||  =  1}. 


1.3  Parametrisation 


For  the  remainder  of  this  report,  the  path  is  considered  to  be  parametrised  by  arc  length 
s,  which  is  more  mathematically  convenient  than  parametrisation  with  respect  to  time. 
When  parametrised  by  s,  the  state  equation  simplifies  to 

yx(s)  =  a(s)  (s  >  0), 

(1) 

yx(o)  =  x. 

The  DPP  becomes 

u(x)  =  jinf^  jy  R(yx(s),  a(s))  ds  +  u(yx(r))l  (0  <  r  <  4(a)),  (DPP) 

where  R(yx(s),  a(s))  =  i?(yx(s),  a(s))  //(yx(s),  a(s))  is  the  running  cost  per  unit  length, 
and  4(a)  is  the  arc  length  associated  with  a  and  x  (shortest  total  arc  length  for  yx(s,  a) 
to  hit  the  target  set).  Finally,  the  HJB  equation  simplifies  to 


min  {a  •  Vii(x)  +  i?(x,  a)}  =  0 
aeA 

u(x)  =  g(x) 


in  D  \  T, 
on  T. 


(HJB) 


1.4  Isotropic  Running  Costs 


If  -R(x,  a)  =  i?(x)  then  the  running  cost  is  isotropic,  and  the  optimal  control  a*  is  the 
direction  of  steepest  descent  of  u.  given  by 


a*(x)  = 


Vm(x) 

l|Vu(x)||' 


(2) 


Equivalently,  a*(x)  is  orthogonal  to  the  level  sets  (iso-cost  contours)  of  tt(x),  if  it(x)  is 
differentiable  at  x.  Equation  (2)  decouples  the  tasks  of  computing  the  optimal  control  and 
the  value  function,  and  results  in  the  HJB  equation  simplifying  to 


l|Vu(x)||  =  i?(x).  (3) 

Equation  (3)  is  known  as  the  Eikonal  equation  and  has  numerous  applications,  for  exam¬ 
ple,  in  path  planning,  computational  geometry,  computer  vision,  and  image  enhancement 
[Sethian  19996]. 

Numerical  methods  for  solving  the  Eikonal  equation  include  Tsitsiklis’  control-theoretic 
algorithm  [Tsitsiklis  1995],  Fast  Marching  Methods  [Sethian  1999a,  Sethian  19996,  Cristiani 
2009],  and  Fast  Sweeping  Methods  [Tsai  et  al.  2003,  Kao,  Osher  &  Tsai  2005,  Qian,  Zhang 
&  Zhao  2007a,  Qian,  Zhang  &  Zhao  20076].  While  these  numerical  methods  can  be  ap¬ 
plied  to  anisotropic  path  planning  problems  for  some  special  cases,  their  application  to 
general  anisotropic  path  planning  problems  is  either  not  valid  or  impractical  [Sethian  & 
Vladimirsky  2003,  Alton  &  Mitchell  2012], 
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2  Ordered  Upwind  Method 

OUM  compute  the  numerical  value  function  U  by  processing  subsets  of  the  computational 
mesh.  These  include  three  disjoint  sets:  where  U  is  known;  where  a  tentative  value  for 
U  has  been  computed  using  an  approximation  to  the  DPP;  and  where  no  information 
regarding  U  is  known.  The  OUM  algorithm  iteratively  updates  these  (and  other)  sets 
until  U  has  been  computed  for  all  points  in  the  computational  mesh. 

This  section  introduces  OUM  by  presenting  a  summary  of  Section  6  from  Sethian  & 
Vladimirsky  [2003],  where  minimum  time  optimal  control  problems  are  considered.4 


2.1  Computational  Mesh 

To  simplify  the  discussion  let  !lcl2,  noting  that  OUM  are  valid  in  Mn.  Let  V/,  c  M2 
be  an  unstructured  triangulated  mesh  of  diameter  h,  where  h  is  the  maximum  distance 
between  any  two  adjacent  mesh  points,  then  adjacent  mesh  points  xy  and  in  V/,  satisfy 
| j x y  —  Xfc||  ^  h.  Define  Sij,  =  fi  O  X \  and  7),  =  T  O  Xy  to  be  discretisations  of  D  and  T . 
respectively.  Since  TcSi  the  computational  mesh  is  given  by  ily . 


2.2  Upwinding  Approximation  to  the  DPP 

Let  x;xxfc  be  a  simplex  with  xy,  x,  x*,  £  such  that  x?-,  xyy  are  adjacent  and  U(x.j),  U (x*,) 
are  known  from  previous  iterations  of  the  OUM.  At  the  heart  of  OUM  is  the  following 
first  order  upwind  approximation  of  the  DPP  in  xyxx/-: 

txjxfc(x)  =  min{r(C)i?(x,  ac)  +  C  Ufa)  +  (1  -  CMxfc)}>  (4) 

where  UXjXfc(x)  is  the  tentative  value  function  in  xyxx*,,  r(£)  =  ||(xy  +  (1  —  C)xfe  —  xl|5 
and  the  control  is  given  by 

0U  +  (1  -  C)xfc  -  x 

Qf  = - V) — ' 

Equation  (4)  can  be  motivated  by  observing  that  as  h  — >  0,  the  optimal  path  at  x 
and  within  xyxx/,.  approaches  a  straight  line  that  will  intersect  the  line  segment  xyx^,  at 
a  point  x  =  (jXj  +  (1  —  C)xfc  for  (  £  [0, 1].  The  value  function  at  x  is  approximated  using 
linear  interpolation: 

«(x)  ~  C«(xj)  +  (1  -  C Mxfc)- 

Furthermore,  as  h  — >  0  the  running  cost  approaches  a  constant  in  XjXXfc.  Applying  these 
approximations  to  the  DPP  leads  to  Equation  (4). 

The  tentative  value  function  V (x)  is  defined  to  be  the  minimum  of  VjCj.Xfc(x)  over  a 
set  of  simplices  obtained  by  varying  xy  and  xy,:;  both  V (x)  and  the  set  of  simplices  are 
updated  at  each  iteration  of  the  OUM.  When  V (x)  becomes  the  minimum  tentative  value 
function  during  the  iterations  of  the  OUM,  then  the  final  value  of  U (x)  is  identified  with 

Ux)- _ 

4The  equivalence  of  minimum  cost  and  minimum  time  optimal  control  problems  is  established  in 
Vladimirsky  [2001,  Section  2.2.5]. 
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CAAAAAAAC 

\  / 

•  C  A  A  A  A  A— A  C  • 

\  / 

•  C  A— A— A  ACC* 

\  / 

•  *CCCAC*  •  • 

•  •••CC*** 

Figure  1:  An  illustration  of  the  Accepted  (A),  AcceptedFront  (A),  Considered  (C)  and 
Far  (dots)  sets.  The  line  segments  of  the  AF  set  are  indicated  by  lines  with  arrowheads. 


2.3  OUM  Sets 

Like  Dijkstra’s  shortest-path  algorithm  for  graphs,  the  mesh  points  in  are  partitioned 
into  three  sets  that  are  updated  at  each  iteration  of  the  OUM: 

Far:  no  information  regarding  U  is  known; 

Accepted:  U  has  been  computed;  and 

Considered:  adjacent  to  Accepted  and  tentative  values,  V,  for  U  have  been  computed. 


OUM  also  update  the  following  sets  at  each  iteration: 


t:  Accepted  mesh  points  that  are  adjacent  to  some  Considered  points;  and 


AcceptedFront: 

AF:  set  of  line  segments  XjXfc,  where  Xj  and  x&  are  adjacent  mesh  points  in  the  Accept¬ 
edFront  such  that  there  exists  a  Considered  point  adjacent  to  both  x?  and  X&. 


The  AF  set  may  be  viewed  as  the  set  of  line  segments  that  constitute  the  boundary  of 
the  accepted  region.  An  illustration  of  the  Accepted,  AcceptedFront,  AF,  Considered  and 
Far  sets  is  shown  in  Figure  1. 

Finally,  the  near  front  set  NF(x)  is  updated  for  each  x  E  Considered  at  each  iteration 
of  the  OUM,  and  is  defined  to  be 


NF(x)  =  {xjXfc  E  AF  |  there  exists  axon  XjX*,  such  that  ||x  —  x||  ^  /iY(x)},  (5) 

where  T(x)  is  known  as  the  anisotropy  function,  which  is  a  local  measure  of  anisotropy  in 
the  running  cost:5 

maxae_4  R(x,  a) 


T(x)  = 


(6) 


rninag^  R(x,  a)  ' 

The  NF(x)  set  is  the  part  of  AF  that  is  relevant  to  x,  that  is,  NF(x)  contains  those  line 
segments  from  which  simplices  are  constructed  with  each  x  E  Considered,  at  which  the 
tentative  value  function  V (x)  is  updated  using  Equation  (4)  at  each  iteration  of  the  OUM. 


5For  isotropic  running  costs  T(x)  =  1. 
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Figure  2:  Definition  diagram  for  illuminating  the  origins  of  the  NF(x)  set. 


The  remainder  of  this  section  is  dedicated  to  illuminating  the  origins  of  the  NF(x)  set: 
NF(x)  is  crucial  to  the  efficient  computation  of  the  numerical  value  function  for  anisotropic 
path  planning  problems. 

Recall  that  for  isotropic  path  planning  problems,  the  optimal  control  a*  and  — Vu  are 
parallel  (see  Section  1.4),  which  is  exploited  to  optimise  the  computational  performance  of 
numerical  methods  for  the  isotropic  case.  Whereas  in  the  anisotropic  case,  a*  and  —  Vu 
are  not  necessarily  parallel.  This  is  the  principal  challenge  for  designing  efficient  numerical 
methods  for  anisotropic  path  planning  problems:  unlike  in  the  isotropic  case,  to  compute 
an  optimal  path  at  x,  it  may  be  necessary  to  use  mesh  points  that  are  not  neighbours  of 


A  significant  contribution  of  Sethian  &  Vladimirsky  [2003]  is  the  determination  of  a 
bound  on  the  angle  between  a*  and  —  Vu.  This  bound  motivates  the  definition  of  the 
NF(x)  set,  and  facilitates  the  efficient  computation  of  the  numerical  value  function  by 
reducing  the  size  of  the  computational  stencil  at  x.6 

To  derive  an  expression  for  the  angle  between  a*  and  — Vu,  consider  a  point  x  on  a 
level  set  of  u  where  Vu  exists,  with  x  at  a  distance  greater  than  h  >  0  from  the  target 
set.  Imagine  a  line  drawn  parallel  to,  and  at  a  distance  of  h  from,  the  tangent  line  to  the 
level  set  at  x.  If  h  is  sufficiently  small,  then  the  optimal  path  at  x  can  be  approximated 
by  a  straight  line  that  passes  through  x,  as  shown  in  Figure  2.  The  angle  between  a*  and 
— Vit  satisfies 

COS  0=7— - 77,  (7) 

||x  —  X  || 

as  indicated  in  Figure  2. 

It  remains  to  eliminate  9  from  Equation  (7).  Following  Vladimirsky  [2001]  and  Sethian 
&  Vladimirsky  [2003],  let  i?i(x)  =  minagv4  R(x,  cc)  and  i?2(x)  =  m&xaeyt  R(x,  £*0)  and  re¬ 
call  that  i?(x,  ct)  >  0.  It  follows  from  the  HJB  equation  that  a*  ■'S7u  <  0  and  consequently 

a*  ■  Vu  a*  ■  Vu 

R(x,a*)  ""  -Ri(x) 

6The  computational  stencil  at  x  is  the  set  of  mesh  points  required  to  compute  the  numerical  solution 
at  x.  The  NF(x)  set  defines  the  computational  stencil  at  x  for  OUM. 
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Also,  for  all  a  e  A, 


a*  ■  Vu  a  ■  Vu 


R(x,  a*)  ^  R(x,  a)  ’ 

since  u  satisfies  the  HJB  equation  and  ad  is  an  optimal  control.  Choosing  a 
in  Equation  (9)  leads  to 

a*-Vu  <  ||Vu|| 


R(x,at*)  i?2(x) 

Combining  Equations  (8)  and  (10)  results  in 

jv«| 


a*  •  (-Vu)  ^ 


T(x)  ’ 


(9) 

— Vu/||Vu|| 

(10) 


where  Y(x)  is  given  by  Equation  (6).  By  definition,  a*  •  (—'Vu)  =  ||  Vu||  cos  9  and  therefore 

cosd^vrv-  (n) 

T(x) 

Equation  (11)  is  the  bound  on  the  angle  between  a*  and  —Vu  reported  in  Vladimirsky 
[2001]  and  Sethian  &  Vladimirsky  [2003].  Finally,  Equations  (7)  and  (11)  yield 


x  —  x||  ^  /iY(x). 


(12) 


Now  let  x  E  Considered.  Returning  to  the  upwinding  approximation  to  the  DPP  (refer 
to  Section  2.2),  it  can  be  seen  from  Equation  (4)  and  Figure  1  that  for  Ux,Xfc(x)  to  be 
well-defined,  xyx*,  E  AF.  Hence  a  candidate  definition  for  the  tentative  value  function 
V(x)  is 

V(x)=  min  UXXfc(x).  (13) 

XjXfcgAF 


Indeed,  an  algorithm  can  be  developed  to  compute  U  based  on  Equation  (13)  such  that 
U  — »  u  as  h  — >  0  [Sethian  &  Vladimirsky  2003];  however,  this  algorithm  will  not  be 
computationally  efficient.  Instead,  if  x  =  ^xy  +  (1  —  C)x^  for  £  E  [0, 1],  then  Equation  (12) 
can  be  employed  to  reduce  the  number  of  elements  in  AF  that  are  required  to  evaluate 
V(x),  that  is, 


V(x) 


min 

XjXfceNFl(x) 


l'x7xfc(x), 


(14) 


which  follows  from  Equation  (5).  OUM  use  this  definition  of  V  to  compute  U. 


Note  that  the  aforementioned  arguments  only  illuminate  the  definition  of  NF(x);  in 
particular,  these  arguments  fail  at  points  where  Vu  does  not  exist.  The  proof  that  Equa¬ 
tions  (13)  and  (14)  result  in  the  same  value  for  U  appears  in  Sethian  &  Vladimirsky 
[2003]. 
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2.4  The  OUM  Algorithm 

The  implementation  of  the  OUM  algorithm  that  is  the  subject  of  this  report  appears  in 
Section  3.4  as  Algorithm  8.  The  OUM  algorithm  from  Sethian  &  Vladimirsky  [2003]  is 
reproduced  here  to  serve  as  an  introduction  and  to  enable  a  comparison  with  Algorithm  8: 

1.  Start  with  all  the  mesh  points  in  Far. 

2.  Move  the  mesh  points  in  the  target  set  x  G  7^  to  Accepted  (t/(x)  =  p(x)). 

3.  Move  all  the  mesh  points  x  adjacent  to  the  target  set  into  Considered  and  evaluate 
the  tentative  values 

V(x):=  min  VXjxfe(x).  (15) 

XjXfc  eNE(x) 

4.  Find  the  mesh  point  x  with  the  smallest  value  of  V  among  all  the  Considered. 

5.  Move  x  to  Accepted  (U(5t)  =  V(x))  and  update  the  AcceptedFront. 

6.  Move  the  Far  mesh  points  adjacent  to  x  into  Considered  and  compute  their  tentative 
values  by  Equation  (15). 

7.  Recompute  the  value  for  all  the  other  Considered  x  such  that  xx;;  e  NF(x) 

U(x)  :=  min  <jV(x) ,  min  Vxx,  (x) ) .  (16) 

(  xx,eNF(x)  J 


8.  If  Considered  is  not  empty,  then  go  to  4. 


The  OUM  algorithm 

•  has  a  computational  complexity  of  0(TMlog(M))  as  h  — »  0; 

•  computes  the  numerical  solution  U  that  converges  to  u  as  h  — ►  0; 

•  is  at  most  first-order  accurate;  and 

•  is  valid  for  anisotropic  path  planning  problems. 


Here  M  is  the  number  of  mesh  points  in  Q /,  and  T  =  maxxGn  Y(x).  This  estimate  of  the 
computational  complexity  counts  the  calculation  of  14:, x,  (x)  via  Equation  (4)  as  a  single 
operation,  since  this  is  performed  independently  of  the  other  mesh  points  in  f 

Note  that  Dijkstra’s  algorithm  has  a  computational  complexity  of  0(Mlog(M)).  How¬ 
ever,  unlike  Dijkstra’s  algorithm,  and  Sethian’s  Fast  Marching  Method  [Sethian  1999a, 
Sethian  19996]  and  Tsitsiklis’  algorithm  [Tsitsiklis  1995],  the  OUM  algorithm  does  not 
add  mesh  points  to  the  Accepted  set  in  the  order  of  increasing  U. 
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3  Implementation 

This  section  details  a  generic  implementation  of  the  OUM  that  has  the  following  charac¬ 
teristics: 

•  Qh  is  constructed  from  a  uniform  mesh  of  equilateral  triangles  on  M2;  and 

•  U  and  V  are  stored  as  functions  of  the  integer  mesh  co-ordinates  to  allow  consistent 
and  efficient  data  retrieval." 

The  requirements  to  use  a  non-uniform  mesh  are  typically  driven  by  features  of  a 
particular  application,  such  as  the  geometry  of  the  domain  and  the  form  of  the  running 
cost,  or  by  the  need  to  improve  computational  performance.  Generating  a  non-uniform 
mesh  requires  a  refinement  condition,  a  refinement  algorithm,  and  data  structures  for 
efficient  storage  and  retrieval  of  geometric  and  topological  properties  of  the  mesh.  For 
OUM,  at  least  the  position  and  adjacency  of  the  mesh  points  must  be  stored. 

A  uniform  mesh  is  considered  in  this  report  to  simplify  the  discussion,  avoid  application 
specific  issues,  and  to  demonstrate  a  concrete  implementation  of  the  OUM.  A  uniform 
mesh  of  equilateral  triangles  was  chosen  because  the  distance  between  adjacent  mesh 
points  is  equal  in  all  directions.  Whereas  for  a  triangulated  mesh  constructed  from  a 
Cartesian  grid  with  spacing  8.  the  triangulation  diameter  is  increased  to  y/28  and  hence 
the  numerical  accuracy  is  reduced.  However,  a  triangulation  based  on  a  Cartesian  grid 
is  easier  to  implement  and  generalise  to  higher  dimensions,  and  many  of  the  standard 
approaches  for  visualising  the  optimal  paths  and  level  sets  are  based  on  Cartesian  grids. 
Hence  an  implementation  of  the  nresh-dependent  functions  for  this  case  has  been  included 
in  Appendix  A. 

For  more  information  on  mesh  construction,  including  mesh  refinement  and  construc¬ 
tion  in  higher  dimensions,  refer  to  Cline  &  Renka  [1984],  Bansch  [1991],  Bey  [1995], 
Maubach  [1995],  Ruppert  [1995],  Arnold,  Mukherjee  &  Pouly  [2000],  Shewchuk  [2002] 
and  Hjelle  &  Dmhlen  [2006]. 

Define  the  set  of  integer  mesh  co-ordinates  H?  to  be 

M h  =  {{i,j)  €  Z2  |  x(i,j)  G  14}, 

where  x(i,  j)  is  a  mesh  point  with  integer  mesh  co-ordinate  (i,j).  Since  U  and  V  are 
stored  as  functions  of  D? ,  for  the  remainder  of  this  report,  the  OUM  sets  introduced  in 
Section  2.3  are  considered  to  be  subsets  of  (or  constructed  from  elements  of  fl|),  and 
hence  the  OUM  algorithm  will  process  these  subsets  to  compute  U. 


3.1  Mesh  Functions 

Functions  that  depend  on  the  structure  of  the  triangulation  are  presented  in  this  section. 
This  comprises  functions  for  transforming  between  mesh  points  x(i,j)  e  D/,  and  mesh  co- 

'  For  example,  U  and  V  could  be  stored  in  an  array,  with  the  integer  mesh  co-ordinates  corresponding 
to  indices  of  the  array.  Alternatively,  some  mathematical  software  packages  allow  functions  to  be  defined 
for  each  discrete  point  in  their  domain;  in  which  case,  these  functions  effectively  behave  like  arrays  with 
arbitrary  indices. 
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ordinates  (i.  j)  £  for  mesh  construction,  and  functions  that  return  mesh  co-ordinates 
corresponding  to  the  neighbours  of  given  mesh  points. 


3.1.1  Mesh  Transformations 


The  mesh  point  x(i,j)  £  Hft  indexed  by  the  mesh  co-ordinate  (i,j)  £  is  given  by8 

x(z,j)  =  xr  +  /i(ief  +  jef),  (17) 

where  x-j  £  int(T)  is  the  target  location  and 

ef  =  (l,0),  e£  =  ±(l,V3),  (18) 

are  the  triangulation  unit  vectors.  Observe  that  =  1/2. 

The  mesh  co-ordinate  (i,j)  £  Of  which  results  in  a  mesh  point  x(i,j)  £  H/,  that  is 
nearest  to  a  given  point  y  £  fl  (not  necessarily  a  mesh  point)  is  obtained  from9 


i  = 


1 ,  ,  i : 

fc(  y-xri-e,-^ 


J  = 


2  1,  ' 

L7!S<y“XT)'e2 


(19) 


Here  [  •  ]  represents  rounding  to  the  nearest  integer,  and  e*  is  the  ith  standard  basis  vector 
in  M2. 

Equations  (17)  to  (19)  enable  transformations  between  subsets  of  and  subsets  of  H 
to  be  defined.  Let  MeshPoint  take  as  inputs  h,  x-j  and  mesh  co-ordinates,  and  return  the 
corresponding  mesh  points,  and  let  Nearestlndex  take  as  inputs  h,  x-q  and  points  in  Q, 
and  return  the  corresponding  (nearest)  mesh  co-ordinates,  that  is: 

MeshPoint  :  M0,1  X  int(T)  X  Uf  — ►  lift, 


Nearestlndex  :  M0,1  X  int(T)  X  H  , 

where  M0,1  =  {x  £  R  |  0  <  x  <  1}.  In  particular,  if  C  and 

MeshPoint  ^/r,  x-j,  Sf^j  =  5ft, 

where  5ft  C  H/j,  then 

Nearestlndex(/i,  X7-,  5ft)  =  Sq. 

However,  if  S  C  Q.  then  in  general 

MeshPoint(/?.,  X7-,  Nearestlndex(/i, X7-,  5))  7^  S, 

as  S  may  contain  elements  that  are  not  mesh  points. 

8The  dependence  of  x(i,  j)  on  the  target  location  and  the  triangulation  diameter  is  suppressed  for 
notational  simplicity. 

9It  can  be  shown  that  ||y  —  x(i,  j)||  is  minimised  for  all  the  combinations  of  rounding  i  and  j  up  and 
down  to  the  nearest  integer,  if  (i,j)  is  given  by  Equation  (19). 
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Algorithm  1:  Construct  O?  where  D  is  a  unit  square  centred  at  xc. 


Input:  Ph{&),  h,  x-j  and  xc  such  that  Hx-j  —  x^^  ^  0.5 

Output: 


2  foreach  (i,j)  G  Z2  such  that  |i|  ^  ph{ D)  and  |j|  ^  Ph{ty  do 

3  if  ||MeshPoint(/i,  x-j,  (i,  j))  —  xc||oo  ^  0.5  then 

4  [_ 


3.1.2  Mesh  Construction 


Mesh  construction  strongly  depends  on  the  geometry  of  D  and  T .  even  for  a  uniform 
mesh,  and  a  general  discussion  of  this  topic  is  beyond  the  scope  of  this  report.  Instead,  a 
construction  where  D  is  a  unit  square  is  presented  here  as  an  example. 

Recall  that  the  OUM  algorithm  processes  subsets  of  to  compute  U .  To  construct 
it  is  necessary  to  determine  the  magnitudes  of  i  and  j  such  that  Xh  covers  D.  To  this  end, 
let  r  be  the  largest  distance  between  any  two  elements  of  D.  Setting  y  —  x-j  =  r(cos  8,  sin  8) 
in  Equation  (19)  leads  to 


i\  ^ 


2  r 

VS  h 


and  \j\  sC 


2  r 

VS  h 


where  [•]  represents  rounding  up  to  the  nearest  integer, 
tional  radius  ph{&)  to  be 


Ph(fy 


2  r(D) 

VS  h 


Therefore,  define  the  computa- 


A  construction  where  fl  is  a  unit  square  centred  at  xc  is  presented  in  Algorithm  1;  in 
this  case  r(Q)  =  V2.  Observe  that  Algorithm  1  can  be  used  to  construct  ilfr  when  D  is  a 
circle  of  radius  1/2  centred  at  xc  by  setting  r(il)  =  1  and  replacing  |j  •  Hoc  with  ||  •  |j. 


3.1.3  Neighbourhood  Functions 

The  notion  of  adjacency  is  central  to  the  OUM  algorithm,  and  therefore  a  function  that 
returns  the  neighbours  of  a  set  is  required.  To  begin,  the  neighbours  of  the  mesh  co¬ 
ordinate  (i,j)  are  given  by10 

N(i,j)  =  {(i  +  1,  j),  (i,j  +  l),(i  -  l,j  +  1),(*  -  1  ~  !),(*  +  1,3  ~  !)},  (20) 

noting  that  (i,j)  0  N(i,j).  The  N(i,j)  set  is  shown  in  Figure  3  mapped  onto  fl/j.  Ob¬ 
serve  that  the  elements  of  N(i,j)  are  arranged  anticlockwise  when  mapped  onto  0/,.  The 
neighbourhood  of  (i,j)  is  given  by 

N(i,j)  =  {( i,j ),  (i  +  l,j),  ( i,j  +  1),  (i  -  1  ,j  +  1  ),(i  -  1  ,j),(i,j  -  1  ),{i  +  l,j  ~  1)}, 

1()If  (i,j)  is  on  the  boundary  of  f then  some  elements  of  N(i,j)  will  not  belong  to  fi*.  This  does  not 
cause  any  difficulties  since  N(i,j)  is  always  intersected  with  a  subset  of  fi*  in  this  report. 
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x(®  -  1,  j  +  1)  •  •  x(i,  j  +  1) 

x(*  -l,j)  m  •  •  x(i  +  1  ,j) 


xu 


LJ  -  1)  •  •  x(i  +  1,  j  -  1) 


Figure  3:  The  N(i,j)  set  shown  mapped  onto  Qh  as  large  dots,  where  the  x(  • ,  •)  are 
given  by  Equation  (17). 


that  is,  N(i,j)  =  N(i,j )  U  {(i,  j)}. 

The  definition  of  N(i,j )  can  be  generalised  to  encompass  sets  by  defining  N(S f)  to  be 

N(Sl)=  (J  N(i,j)\Sf  (21) 

for  Sj‘  C  flfo.  In  this  report,  N(Sf)  is  constructed  using 

N(Sl)=  |J  N(i,j). 

(hl)esl 

This  results  in  N(Sff)  containing  elements  of  S(\  which  is  inconsistent  with  the  definition 
provided  by  Equation  (21).  However  this  does  not  cause  any  problems,  because  if  Sj‘  is  a 
singleton  set  then  the  correct  result  is  obtained,  otherwise  N(Sf)  will  always  be  intersected 
with  a  set  that  is  disjoint  to  Sf  in  this  report. 


3.2  Set  Construction 

This  section  is  devoted  to  the  construction  of  the  OUM  sets,  which  were  introduced  in 
Section  2.3. 


3.2.1  Adjacency  Function 

To  reiterate,  the  notion  of  adjacency  is  central  to  the  OUM  algorithm.  A  mesh  co-ordinate 
(i,  j)  is  defined  to  be  adjacent  to  a  set  Bf  c  if  (i,  j)  6  N(Bf),  where  N(  ■ )  is  given  by 
Equation  (21).  The  adjacency  function  AdjacentTo(A|,  Bf  )  is  then  defined  to  be  the  set 
of  mesh  co-ordinates  in  Aft  C  that  are  adjacent  to  the  mesh  co-ordinates  in  B 

AdjacentTo(Af ,  Bf)  =  Af  n  N(B%).  (22) 

The  AdjacentTo  function  is  required  to  construct  most  of  the  OUM  sets. 
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Algorithm  2:  AFAdjacentPairs 


Input:  AcceptedFront  and  Considered 

Output:  AF 


1 

2 

3 

4 

5 

6 

7 

8 
9 

10 

11 


AF  <—  0 

if  Considered  ^  0  then 
S  <—  AcceptedFront 
while  S'  /  0  do 
select  (i,j)  £  5 
S<-S\{(z,j)} 

A  <—  AdjacentTo^,  {(z,  j)}) 

IVjj  <—  Considered  D  N(i,j) 
foreach  (k,  l)  £  A  do 

if  Nij  Cl  N(k,  l)  7^  0  then 

L  AF<-AFU  {{(*,./),  (M)}} 


3.2.2  AcceptedFront  Construction 

By  definition,  the  AcceptedFront  can  be  constructed  using 

AcceptedFront  =  AdjacentTo (Accepted,  Considered).  (23) 

However  this  is  computationally  inefficient,  since  changes  to  the  AcceptedFront  between 
iterations  of  the  OUM  algorithm  only  occur  within  a  neighbourhood  of  the  most  recently 
accepted  mesh  co-ordinate  (i,j).  Consequently,  after  the  AcceptedFront  has  been  ini¬ 
tialised  on  the  mesh  co-ordinates  in  the  target  set,  it  is  then  updated  according  to 

AcceptedFront  <—  ( AcceptedFront  \  N(i,j ))  U  ^ 

AdjacentTo(Acceptedn  N(i,j ),  Considered) . 

Equation  (24)  is  verified  in  Appendix  B. 

The  update  rule  given  by  Equation  (24)  adds  new  elements  to  the  AcceptedFront  by 
calling  AdjacentTo  to  operate  on  Accepted  Cl  N(i,  j),  which  has  (at  most)  seven  elements. 
Compare  this  with  Equation  (23),  where  the  AcceptedFront  is  constructed  from  the  entire 
Accepted  set,  which  may  have  tens  of  thousands  of  elements.11 


3.2.3  AF  Construction 

Recall  from  Section  2.3  that  the  AF  set  is  given,  in  terms  of  mesh  co-ordinates,  by 

AF  ={{(i,j),(k,l)}  £  AcceptedFront  x  AcceptedFront  \ 

j|x(i,  j)  —  x(fc,  l) ||  =  h  and  Considered  Cl  N(i,j)  Cl  N(k,  l)  /  0}. 

11The  Accepted  set  grows  by  one  element  per  iteration  of  the  OUM  algorithm  until  Accepted  = 
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Algorithm  3:  AFUpdate 


Input:  AF,  AcceptedFront,  Considered,  and  N(i,j),  where  (i,j)  is  the  most 
recently  accepted  mesh  co-ordinate 
Output:  updated  AF 


2  foreach  {i,  j)  G  N(i,j)  do 


A  «-  AdjacentTo(IV(i, 
foreach  (k,  l)  G  A  do 

L  p(hi)  p{hj)  u{{(lJ)>(M)}} 


6  AF  4—  (AF\  P(i,j ))  U  AFAdj acentPair s( AcceptedFront  fl  N(i,j),  Considered ) 


Two  functions  are  employed  to  construct  AF:  AFAdj  acentPairs  constructs  the  entire  AF, 
and  AFUpdate  updates  AF  at  step  5  of  the  OUM  algorithm;  refer  to  Section  2.4. 

AFAdj  acentPairs  takes  the  AcceptedFront  and  Considered  sets  as  inputs  and  returns 
the  entire  AF,  and  is  defined  in  Algorithm  2.  AFAdj  acentPairs  produces  the  AF  set  with 
minimal  cardinality,  meaning  that  if  {(i,j),(k,l)}  G  AF  then  {(k,l),(i,j)}  0  AF,  which 
is  permitted  since  x.;xy  is  equivalent  to  xyXjj  this  is  achieved  at  line  6  of  Algorithm  2. 
AFAdj  acentPairs  can  be  exclusively  used  to  construct  AF.  However,  the  computational 
performance  of  AF  construction  can  be  dramatically  improved  by  noting  that  elements 
are  only  added  to  (or  removed  from)  the  AcceptedFront  from  a  neighbourhood  of  the  most 
recently  accepted  mesh  co-ordinate  (i,  j). 

This  is  exploited  in  the  definition  of  AFUpdate,  which  updates  AF  at  step  5  of  the 
OUM  algorithm,  and  is  presented  in  Algorithm  3.  The  purpose  of  lines  2-5  of  Algorithm  3 
is  to  construct  the  set  which  consists  of  all  adjacent  pairs  of  mesh  co-ordinates 

with  elements  in  N(i,j),  including  elements  that  no  longer  belong  to  AF.  P(i,j)  is  given 
by12 

=  {{(*,  j),  (M)}  e  N(i,j)  x  N(i,j)  I  ||x(i,  j)  -  x(fc,/)l|  =  h}. 

AFUpdate  adds  new  elements  to  AF  by  calling  AFAdj  acentPairs  to  operate  on  the  set 
AcceptedFront <1  N(i,  j) ,  which  has  (at  most)  seven  elements,  whereas  AcceptedFront  may 
have  thousands  of  elements  at  each  iteration  of  the  OUM  algorithm.  The  validity  of 
AFUpdate  can  be  established  by  comparing  line  6  of  Algorithm  3  to  the  AcceptedFront 
update  rule  given  by  Equation  (24). 


3.2.4  NF (i,j)  Construction 

At  step  7  of  the  OUM  algorithm  in  Section  2.4,  the  tentative  value  function  is  recomputed 
for  each  (i,  j)  G  Considered  such  that  •}  G  NF(i,j),13  where  (i,  j)  G  Accepted  is 

the  most  recently  accepted  mesh  co-ordinate.  Recall  from  Equation  (5)  that,  in  terms  of 

12 Note  that  if  {(*,  j),  (k,  l)}  £  P(i,j)  then  {(k,  l),  (i,  j)}  £  P(i,j)- 

13For  greater  clarity,  this  should  read  {(i,j),  ■  }  £  NF(i,  j)  or  {  ■ ,  (i,j)}  £  NF(i,  j). 
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Algorithm  4:  AFSubset 

Input:  AF,  and  (i,j)  corresponding  to  x  in  Section  2.4 
Output:  subset  of  AF  containing  {  • ,  or  {(i,j),  •  } 

1  5^0 

2  N  <—  N(J,]) 

3  foreach  ( k ,  l)  £  N  do 

4  |_  S  <-  S(J  {{(k,l),(i,j)}  ,{(i,j),(k,l)}} 

5  AFnS 


mesh  co-ordinates,  the  near  front  NF(*,j)  is  defined  to  be 
NF(i,  j)  ={{(k,l),(m,n)}  e  AF  \ 

there  exists  axon  x(/c,  l)  x(m,  n)  such  that  ||x  —  x(i,  j)||  ^  hT(x(i,j))J. 

(25) 

The  near  front  is  constructed  once  per  element  of  the  Considered  set,  which  may 
contain  thousands  of  elements,  for  each  iteration  of  the  OUM  algorithm.  Therefore  im¬ 
proving  the  efficiency  of  near  front  construction  will  greatly  improve  the  overall  com¬ 
putational  performance  of  the  OUM  algorithm.  To  this  end,  at  step  7,  instead  of  con¬ 
structing  NF (i,j)  from  the  entire  AF  and  then  extracting  the  subset  of  NF (i,j)  such  that 
{(i,j),  • }  £  NF it  is  more  efficient  to  construct  NF (i,j)  from  the  subset  of  AF  that 
contains  (i,  j)  in  one  of  its  mesh  co-ordinate  pairs.  The  AFSubset  function  takes  as  inputs 
AF  and  and  returns  the  subset  of  AF  containing  {  • ,  ( i,j )}  or  {(i,j),  ■  };  see  Algo¬ 
rithm  4.  Although  AF  will  only  contain  {  • ,  or  ■  },  it  is  not  possible  to  know 

which  one  in  advance;  this  is  taken  into  account  on  line  4  of  Algorithm  4. 

Now  all  that  remains  is  to  derive  a  function  to  apply  the  conditions  in  Equation  (25) 
to  determine  which  elements  of  AF  also  belong  to  NF (i,j).  To  begin,  let  the  quadratic 
polynomial  p(Q  be  defined  by 

p{ 0  =  ||Cx(M)  +  (1  -  C )x(m,n)  -  x(i,j)  |j2  -  (hT(x(i,j)))2,  (26) 

for  (i,j)  £  Considered  and  {(k,l),(m,n)}  £  AF,  where  the  line  segment  x(k,l)  x(m,n)  is 
given  by  Cx(^5  0  +  (1  ~  C)x(mi n )  f°r  C  £  [0, 1].  Therefore,  if  there  exists  a  (  £  [0, 1]  such 
that 

p( 0  <  0,  (27) 

then  {( k ,  l),  (m,  n)}  £  NF 

Let  A (p)  be  the  discriminant  of  p(C)-  The  following  statements  are  properties  of  p(C)' 

•  p"  =  2 h2  >  0,  and  hence  p(C)  is  a  convex  function. 

•  If  A (p)  <  0,  then  p(Q  >  0  for  all  and  hence  Equation  (27)  is  not  satisfied. 

•  If  A (p)  ^  0,  then  p(argmin(p(C)))  ^  0.  Therefore  if  argmin(p(£))  £  [0,1],  then 
Equation  (27)  is  satisfied. 
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Algorithm  5:  NearFrontTest 

Input:  h,  {( k,l),(m,n )}  G  AF,  ( i,j )  G  Considered,  and  T(x(i,j)) 
Output:  True  if  {(k,  l),  (m,  n)}  G  NF otherwise  False 

1  Ci  «—  m  —  i 

2  C2  <-  n  -  j 

3  C3  «—  k  —  m 

4  C4  <—  l  —  n 

5  argmin(p(0)  -  -|(2CiC3  +  C1C4  +  C2C3  +  2C2C4) 

6  p"  <—  2/i2 

7  p'(0)  « - p//argmin(p(C)) 

8  P( 0)  <-  |  (3p"C|  +  p"  (2Cr  +  C2)2  -  8(/iT(x(i,  j)))2) 

9  p(l)  «-  lp"  +  p\ 0)  +p(0) 

10  A(p)  <—  (j/(0))2  —  2p"p(0) 

11  if  A (p)  <  0  then 

12  |  return  False 

13  else  if  p( 0)  ^  0  or  p(l)  ^  0  then 

14  |  return  True 

15  else  if  0  ^  argmin(p(£))  ^  1  then 

16  |  return  True 

17  else 

18  return  False 


.  If  A  (p)  ^  0,  argmin(p(C))  <  0  and  p(0)  >  0,  then  p(Q  >  0  for  (  G  [0,1]  and 
Equation  (27)  is  not  satisfied. 

•  If  A (p)  ^  0,  argmin(p(<C))  >  1  and  p(  1)  >  0,  then  p(()  >  0  for  (  G  [0,1]  and 
Equation  (27)  is  not  satisfied. 

Also,  since  p(Q  is  a  quadratic  polynomial, 

A(p)  =  (p'(0))2  -  2p"p(0), 

argmin(p(C))  = 

p 

p{  1)  =  t^p"  +P{  0)  +p(0). 

Finally,  using  Equations  (17)  and  (18)  and  the  definition  of  p(()  leads  to 
P( 0)  =  h2  ((m  —  i)2  +  (n  —  j)2  +  (m  —  i)(n  —  j))  -  (hT(x(i,j)))2, 

p( 0)  =  2 h2  ((m  —  i)(k  —  m)  +  - (m  —  i)(l  —  n)  +  -(n  —  j)(k  —  m )  +  (n  —  j){l  —  n)  \  . 
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Algorithm  6:  NearFront 


Input:  h.  xr,  AF,  and  ( i,j )  G  Considered 
Output:  NF(i,j) 


1 

2 

3 

4 

5 


NF(i,  j)  <—  0 

C  <—  T(MeshPoint(Ii,  xr,  (i,  j))) 
foreach  {(A:,  /),  (to,  n)}  G  AF  do 

if  NearFrontTest(/i,  {(k,  l ),  (to,  n)},  (z,i),  C)  then 

L  NF(i,j)  ^NF(i,i)U{{(fe,0,(m,fl)}} 


These  properties  of  p(£)  are  incorporated  into  the  definition  of  NearFrontTest,  which 
returns  “True”  if  an  element  of  AF  is  also  an  element  of  NF(z,  j),  or  otherwise  returns 
“False”;  see  Algorithm  5. 14  NearFrontTest  is  a  major  contributor  to  the  total  run-time 
of  the  OUM  algorithm,  due  to  the  number  of  calls  to  NearFrontTest.  Therefore  even 
small  improvements  to  the  efficiency  of  NearFrontTest  may  result  in  large  improvements 
to  the  computational  performance  of  the  OUM  algorithm. 

NF (i,j)  can  now  be  constructed  by  calling  NearFrontTest.  This  is  performed  by  the 
NearFront  function,  which  is  defined  in  Algorithm  6. 


3.3  Tentative  Value  Function 

This  section  concerns  the  evaluation  of  the  tentative  value  function  V  in  Equation  (15) 
of  the  OUM  algorithm  presented  in  Section  2.4.  To  evaluate  the  tentative  value  function, 
two  minimisations  are  performed:  locally  within  each  simplex  constructed  from  the  near 
front,  and  then  the  minimum  of  these  values  is  selected. 


3.3.1  Local  Minimisation 

To  evaluate  V ,  it  is  first  necessary  to  determine  UxiXfe(x)  by  performing  the  function 
minimisation  in  Equation  (4).  This  local  minimisation  is  performed  using  an  algorithm 
that  emulates  the  Golden  Section  Search  routine  from  Press  et  al.  [1992], 

The  Golden  Section  Search  algorithm’s  inputs  include  a  numerical  precision  and  an 
initial  bracket  for  the  minimising  abscissa.  The  numerical  precision  of  this  algorithm  is 
set  to  h  in  our  implementation.  This  incurs  an  0(h2)  numerical  error  in  Ix.Xfc(x),  which 
is  acceptable  as  OUM  are  at  most  first-order  accurate.15 

The  routine  for  initially  bracketing  a  minimum  presented  in  Press  et  al.  [1992]  is  for 
functions  defined  on  an  unbounded  interval,  which  is  not  the  case  here.  This  bracket¬ 
ing  routine  has  been  modified  to  restrict  the  domain  to  [0, 1]  in  our  implementation  of 

14As  x(i,j)  and  x(m,  n)  are  not  necessarily  adjacent  (see  Section  2.2),  Ci  and  C2  in  Algorithm  5  may  be 
large  and  therefore  it  is  desirable  to  avoid  squaring  these  terms.  Consequently,  on  line  8  of  Algorithm  5, 
p(0)  is  expressed  in  a  form  to  enable  cancellation  between  Ci  and  C2  to  occur  before  these  terms  are 
squared. 

15This  follows  from  the  uniform  Lipschitz  continuity  of  U  [Sethian  &  Vladimirsky  2003]. 
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the  OUM.16  As  in  Press  et  al.  [1992],  the  resulting  bracketing  algorithm  is  somewhat 
“formidable”,  and  hence  it  is  not  included  in  this  report.  Furthermore,  experience  sug¬ 
gests  that  the  results  produced  by  the  OUM  algorithm  when  using  this  bracketing  algo¬ 
rithm  compared  with  simply  providing  the  Golden  Section  Search  algorithm  with  an  initial 
bracket  of  (0,  0.5, 1),  are  numerically  indistinguishable  for  the  examples  considered  in  this 
report.  This  is  not  surprising  because  as  h  — ►  0,  the  function  in  the  braces  in  Equation  (4) 
will  approach  a  straight  line  within  the  simplex  xyxxj.,  for  sufficiently  smooth  running 
costs.  Therefore  if  h  is  sufficiently  small,  then  [0, 1]  will  contain  a  unique  minimum  and 
(0,  0.5, 1)  will  be  adequate  to  determine  the  minimising  abscissa.  However,  the  impact  of 
this  bracketing  algorithm  on  the  OUM  algorithm’s  run-time  also  appears  to  be  negligible 
and,  for  highly  oscillatory  running  costs  in  particular,  it  may  provide  some  advantages  by 
quickly  reducing  the  width  of  the  bracket  passed  to  the  Golden  Section  Search  algorithm. 


3.3.2  Evaluation 

Let  Uyz(x,  £)  be  the  function  in  the  braces  in  Equation  (4),  namely, 

Eyz(x,C)  =  r(C)i?(x,ac)  +  C U(k,l)  +  (1  -  C )U(m,n)  ,  (28) 

where  y  =  x(fc, l),  z  =  x(m,n),  r( Q  =  ||(y  +  (1  —  ()z  —  x||,  and  the  control  is  given  by 

Cy  +  (i  -  C)z  -  x 
= - W) - ■ 

The  notation  here  is  somewhat  awkward  since  U  is  stored  as  a  function  of  the  mesh  co¬ 
ordinates  in  fl|,  whereas  the  other  functions  in  Equation  (28)  depend  on  the  mesh  points 
in  nh. 

The  evaluation  of  V  in  Equation  (15)  is  performed  by  TentativeValueFunction,  which 
takes  as  inputs  h,  x-j,  NF (i,j)  and  (i,j)  G  Considered,  and  returns  {(i,j),  V(i,j),  a(i,j)}-, 
see  Algorithm  7.  Note  that,  in  practice,  lines  8  and  9  of  Algorithm  7  would  be  performed 
in  a  single  operation. 

TentativeValueFunction  is  a  major  contributor  to  the  total  run-time  of  the  OUM 
algorithm,  due  to  the  number  of  calls  to  TentativeValueFunction  and  the  function  min¬ 
imisation  on  line  8  of  Algorithm  7.  Therefore  even  small  improvements  to  the  efficiency 
of  TentativeValueFunction  have  the  potential  to  result  in  large  improvements  to  the 
computational  performance  of  the  OUM  algorithm. 


16  Our  algorithm  uses  parabolic  extrapolation  to  attempt  to  bracket  the  minimising  abscissa,  and  if  this 
fails,  then  the  algorithm  steps  downhill  while  remaining  inside  [0, 1]  and  tries  again.  If  this  procedure  fails 
after  three  attempts,  then  the  last  attempt  is  returned. 
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Algorithm  7:  TentativeValueFunction 

Input:  h,  X7-,  NF and  (i,j)  G  Considered 
Output:  {(i,j),V(i,j),a(i,j)} 

1  if  NF(i,j)  =  0  then 

2  |  return  {(i,j),  00,  •  } 

3  else 

4  x  <—  MeshPoint(/i,  x^,  [i,  j)) 

5  V(i,j)<-  00 

6  foreach  {(k,  l),  (m,  n)}  G  NF(i,j)  do 

7  {y ,  z}  <—  MeshPoint(/i,  X7-,  {(k,  l ),  (m,  n)}) 

8  C  «-  minCe[0il]  Uyz(x,  C) 

9  C  argminC6[0)1]  Uyz(x,  C) 

10  if  C  <  V(i,j)  then 

11  V(i,j)^C 

12  |_  Oc(i,  j) 

13  return  {(i,  j),  U(z,  j),  a(i,i)} 
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3.4  The  OUM  Algorithm  Revisited 

Before  revisiting  the  OUM  algorithm,  a  number  of  symbols  still  require  definitions.  Let 

be  the  set  of  mesh  co-ordinates  corresponding  to  the  discretisation  of  T . 

T*={(i,j)eZ2  |  x(i,j)  €  Thj, 
noting  that  C  fif .  Also  let  T(i,j)  be  the  triplet 

T{i,j )  =  {(i,j),V(i,j),a(i,j)}, 

for  (i.  j)  e  Considered,  and 

T(i,j)  =  {(i,j),U(i,j),a*(i,j)}, 

for  (i,  j)  e  Accepted. 

The  OUM  algorithm  that  is  the  subject  of  this  report  is  presented  in  Algorithm  8. 
The  lines  that  appear  as  comments  in  Algorithm  8  correspond  to  the  steps  of  the  OUM 
algorithm  that  was  reproduced  from  Sethian  &  Vladimirsky  [2003]  in  Section  2.4. 

Note  that 

•  the  running  cost  R(  ■ ,  • )  and  anisotropy  function  T(  • )  are  assumed  to  be  strictly 
positive  Lipschitz-continuous  functions;  the  terminal  cost  g(-)  ^  0  is  also  assumed 
to  be  Lipschitz  continuous; 

•  the  dependences  of  NearFront  on  Y(  • )  and  of  TentativeValueFunction  on  R(  ■ ,  • ) 
have  been  suppressed  for  notational  simplicity; 

•  Uf  and  T}f  are  inputs,  and  hence  must  be  determined  by  an  algorithm  such  as 
Algorithm  1,  or  by  some  other  means; 

•  on  line  5,  the  optimal  control  is  not  specified  on  however  it  can  be  specified  as 
a  boundary  condition  on  the  velocity  of  the  entity; 

•  the  AcceptedFront  is  initialised  on  the  Accepted  set  on  line  8; 

•  the  Considered  set  is  required  to  be  sorted  according  to  the  values  of  V  to  determine 
the  minimum  on  line  14;  this  can  be  accomplished  via  a  standard  sorting  algorithm, 
such  as  in  Press  et  al.  [1992],  or  by  using  a  “sort”  function  included  in  most  mathe¬ 
matical  software  packages;17 

•  in  practice,  the  solution  set  S  is  exported  as  a  data  stream  on  line  15  and  stored  in 
a  file; 

•  V  is  initialised  on  the  new  mesh  co-ordinates  in  Considered  on  lines  21  to  25;  and 

•  on  lines  26  to  31,  TentativeValueFunction  is  called  and  V  is  possibly  updated 

to  account  for  the  changes  to  the  near  front  that  result  from  accepting  and 

therefore  Z  is  a  tentative  value  for  V. 

17Sorting  the  Considered  set  contributes  the  log(M)  factor  in  the  computational  complexity  of  the  OUM 
algorithm  [Sethian  &  Vladimirsky  2003]. 
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Algorithm  8:  OrderedUpwindMethod 

Input:  R(  ■ ,  • ),  g(  • ),  T(  • ),  h,  xT,  fif ,  and 

Output:  solution  set  S  with  elements  T(i,j )  =  {(i,j),U(i,j),a*(i,j)}  for  each 

1  5^-0 

//  step  1 

2 

//  step  2 

3  Accepted  4—  7^ 

4  foreach  (i,  j)  G  Accepted  do 

5  |_  5  <-  5u{{(i,j),5(MeshPoint(/i,xT,(i,j))),  •}} 

//  step  3 

6  Considered  <—  AdjacentTo(Far,  Accepted) 

7  Far  <—  Far  \  Considered 

8  AcceptedFront  4—  Accepted 

9  AF  4  AFAdjacentPairs(AcceptedFront,  Considered ) 
io  foreach  (i.  j)  G  Considered  do 

n  NF(i,j)  4—  NearFront(/i,  x^,  AF, 

12  T(i,j)  4—  TentativeValueFunction(/i,  x^,  NF(i,  j),  (i,  j)) 

13  while  Considered  ^  0  do 

//  step  4 

14  (i,  j)  4-  argmin(j J) g Considered  V(i,j) 

15  S<-S_U{T(iJ)} 

16  A  <—  N(i,j ) 

//  step  5 

17  Accepted  4—  Accepted  U  {(i,  j)} 

18  Considered  4—  Considered  \  { (i,  j)} 

19  AcceptedFront  <—  ( AcceptedFront  \N)  U  Adj  acentTo(  Accepted Cl  N,  Considered ) 

20  AF  4—  AFUpdate(AF,  AcceptedFront,  Considered,  N ) 

//  step  6 

21  NewConsidered  4—  AdjacentTo(Far,  {(*,  j)}) 

22  Far  4—  Far  \  NewConsidered 

23  foreach  (i,j)  G  NewConsidered  do 

24  NF(i,j)  4— NearFront(/r,  X7-,  AF,  (i,j)) 

25  T{i,j)  4—  TentativeValueFunction(/i,  X7-,  NF(i,  j),  (i,  j)) 

//  step  7 

26  AF  4—  AFSubset(AF,  ( i,j )) 

27  foreach  (i,j)  G  Considered  do 

28  NF(i,j)  4— NearFront(/i,  X7-,  AF,  (i,j)) 

29  {(*,  j),  Z,  oc(i,j)}  4—  TentativeValueFunction(/i,  x-j,  NF(i,  j),  ( i,j )) 

30  if  Z  <  V(i,j)  then 

31  |_  T(i,j)  4-  {(i,j),Z,a(i,j)} 

32  Considered  4—  Considered  U  NewConsidered 
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3.5  Visualisation 

OrderedUpwindMethod  (Algorithm  8)  returns  {(i,j),U(i,j),a*(i,j)}  for  each  (i,j)  £ 
which  is  typically  stored  in  a  file.  This  data  can  then  be  processed  to  visualise  the  solution 
and  determine  other  properties.  One  method  for  visualising  the  solution  is  to  reconstruct 
the  optimal  paths  starting  at  given  initial  positions  to  the  boundary  of  the  target  set. 
Another  visualisation  is  obtained  by  generating  the  level  sets  of  the  value  function. 

3.5.1  Path  Reconstruction 

Two  approaches  can  be  taken  to  reconstruct  the  optimal  paths  from  data  produced  by 
OrderedUpwindMethod.  Both  commence  with  a  given  initial  position,  and  require  in¬ 
terpolation  to  form  approximations  at  points  in  the  domain  that  are  not  mesh  points. 
Subsequent  points  on  the  optimal  path  are  determined  by  either 

•  interpolating  the  value  function  data  and  performing  a  local  minimisation  (the  value 
function  is  strictly  monotone  decreasing  on  an  optimal  path);  or 

•  interpolating  the  optimal  control  data  and  numerically  integrating  the  state  equation 
given  by  Equation  (1). 

The  second  approach  is  taken  in  this  report. 

The  algorithm  used  in  this  report  for  reconstructing  an  optimal  path  starting  at  a 
given  initial  position  x  £  fl  is  as  follows: 

1.  Set  the  first  point  on  the  optimal  path  yx  to  be  x. 

2.  Find  the  vertices  of  the  simplex  that  contains  yx. 

3.  Approximate  the  optimal  control  at  yx  by  interpolating  the  optimal  control  data  on 
the  vertices  of  the  simplex  that  contains  yx. 

4.  Calculate  the  next  point  on  the  optimal  path  by  numerically  integrating  the  state 
equation,  and  set  yx  to  be  this  point. 

5.  If  yx  is  not  within  a  step  size  of  the  target  set,  then  return  to  step  2  and  continue, 
otherwise  return  the  optimal  path. 

Step  2  of  the  path  reconstruction  algorithm  is  performed  by  the  FindS implex  func¬ 
tion,  which  is  defined  in  Algorithm  9  and  motivated  by  the  definition  diagram  shown  in 
Figure  4. 18  First,  let  xc  =  (xc,yc)  £  Qh  be  the  closest  mesh  point  to  a  given  point  x  £  0, 
and  construct  the  six  candidate  simplices  with  adjacent  vertices  from  the  neighbours  of  xc. 
Then  determine  which  quadrant  of  the  (xc,  yc )  co-ordinate  system  contains  x;  the  number 
of  candidate  simplices  will  now  be  two.  Finally,  the  simplex  that  contains  x  is  determined 
by  considering  the  cosine  of  the  angle  between  x  —  xc  and  x,;  —  xc,  where  i  £  {1,4},  noting 
that  (xj_|_i  —  xc)  •  (xj  —  xc)  =  0.5  for  i  £  {1, . . . ,  5}. 

1;,FindSimplex  is  a  mesh-dependent  function,  and  therefore  an  implementation  of  this  function  for  the 
case  of  a  triangulated  mesh  constructed  from  a  Cartesian  grid  has  been  included  in  Appendix  A. 
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Figure  f:  Definition  diagram  for  the  FindSimplex  function  (Algorithm  9),  showing  the 
neighbours  ofxc  =  (xc,yc)  G  Dh  and  an  example  where  the  simplex  xcx 3X4  contains  it  E  H. 


An  approximation  to  the  optimal  control  at  x  £  H  is  calculated  using  linear  interpola¬ 
tion.  First,  InterpolationCoef  f  icients19  (see  Algorithm  10)  determines  the  barycentric 
co-ordinates  {Cl ,  C2 ,  C3}  C  M3  of  x  with  respect  to  the  vertices  {u,v,w}  C  H))  of  the  sim¬ 
plex  that  contains  x,  by  solving  the  system 

Ci  +  C2  +  C3  =  1, 

Ciu  +  C2V  +  C3W  =  x. 

Then  step  3  of  the  path  reconstruction  algorithm  is  performed  by  InterpolatedControl 
(see  Algorithm  11),  which  takes  the  optimal  control  data,  h,  x-j-  and  x  as  inputs,  then 
calls  FindSimplex  and  InterpolationCoef f  icients,  returning  the  linearly  interpolated 
control  at  x. 

Step  4  of  the  path  reconstruction  algorithm  is  performed  using  Heun’s  method  to  nu¬ 
merically  integrate  Equation  (1).  Heun’s  method  is  a  second  order  Runge-Kutta  method, 
and  is  often  referred  to  as  the  improved  Euler’s  method  [Leader  2004], 

The  path  reconstruction  algorithm  is  implemented  in  Algorithm  12,  which  defines  the 
OptimalPath  function.  OptimalPath  takes  as  inputs  the  optimal  control  data,  7)t,  h,  X7-, 
and  the  initial  position  x  G  fb  OptimalPath  calls  InterpolatedControl,  numerically 
integrates  Equation  (1)  using  Heun’s  method  with  a  step  size  of  h,  and  then  returns  an 
ordered  list  of  points  on  the  optimal  path  with  initial  position  x.  Note  that 

•  the  distance  function  dist(  • ,  S)  :  fl  — >  M  (appearing  on  line  3  of  Algorithm  12)  is 
defined  to  be 

dist(x,  S)  =  min  {||x  —  y||  |  y  G  S}, 
for  a  given  nonempty  finite  set  Sell; 

•  a  different  numerical  method  for  integrating  the  state  equation  can  be  used  by  mod¬ 
ifying  lines  4  to  6  of  Algorithm  12;  and 

19InterpolationCoeff  icients  is  a  mesh-dependent  function,  and  therefore  an  implementation  of  this 
function  for  the  case  of  a  triangulated  mesh  constructed  from  a  Cartesian  grid  has  been  included  in 
Appendix  A. 
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Algorithm  9:  FindSimplex 
Input:  h.  X7-,  and  x£ll  where  x  =  (xi,xz) 

Output:  {u,  v,w}  C  where  uvw  is  a  simplex  with  adjacent  vertices  that 
contains  x 

1  {xc,  xi,  X2,  X3,  X4,  X5,  xg}  <—  MeshPoint  [h,  X7-,  IV(NearestIndex(/i,  X7-.  x))) 

2  if  x'2  >  Uc  then 

3  if  xi  >  xc  then 

4  if  (x  —  xc)  •  (xi  —  xc)  >  0 . 5 /?  1 1 x  —  xc||  then 

5  |  return  {xc, xi, X2} 

6  else 

7  |_  return  {xc,x2,x3} 

8  else 

9  if  (x  —  xc)  •  (X4  —  xc)  >  0.5/i||x  —  xc||  then 

10  |  return  {xc, X3,  X4} 

11  else 

12  |_  return  {xc, X2,  X3} 

13  else 

14  if  x\  >  xc  then 

15  if  (x  —  xc)  •  (xi  —  xc)  >  0.5/i||x  —  xc||  then 

16  |  return  {xc, x@, xi} 

17  else 

18  |_  return  {xc,x5,x6} 

19  else 

20  if  (x  —  xc)  •  (X4  —  xc)  >  0.5/i||x  —  xc||  then 

21  |  return  {xc, X4,  X5} 

22  else 

23  return  {xc, X5,  xg} 


•  the  discretisation  of  the  target  set  can  be  deduced  from  the  solution  data  if,  for 
instance,  the  terminal  cost  is  constant  on  the  boundary  of  the  target  set;  if  this  is 
the  case  then  7/(  is  not  required  as  an  input. 
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Algorithm  10:  InterpolationCoef f icients 

Input:  h.  x  G  D,  and  {u,v,  w}  C  D?,  where  uvw  is  a  simplex  with  adjacent 
vertices  that  contains  x 

Output:  {Ci,  C 2,  C3}  U  M3,  which  are  barycentric  co-ordinates  of  x  with  respect  to 
{u,  v,  w}  such  that  Cl  +  C2  +  C3  =  1 

1  C  <-  2/(3 h2) 

2  Z  4—  X  —  w 

3  C\  < —  Cz  •  (u  —  v) 

4  C'2  4 —  Cz  •  (u  —  w) 

5  C3  4—  Cz  ■  (v  —  w) 

6  {Ci,  C2,  Cs}  -  {Cl  +  c2,  C3  -  Cl,  1  -  C2  -  C3} 


Algorithm  11:  InterpolatedControl 

Input:  for  each  (i,  j)  G  h,  xr,  and  xGH 

Output:  a  linearly  interpolated  control  a  at  x 

1  {u,  v,  w}  FindSimplex(/i,  x-j,  x) 

2  ( k ,  ^),  (to,  n)}  4—  Nearest  Index(/i,  x-j,  {u,  v,  w}) 

3  {Cl,  C2,  Cs}  InterpolationCoef  ficients(/i,  x,  {u,  v,  w}) 

4  a  <—  Cia*(i,  j)  +  C2«*(M)  +  C3 a*(m,n) 

5  ci  <—  a/||a:|| 


Algorithm  12:  OptimalPath 

Input:  for  each  (i,  j)  G  7/,  h, 

x^,  and  xGil 

Output:  an  ordered  list  L  of  points  on  the  optimal  path  with  initial  position  x, 

that  is,  L  =  {x,  yx(si),  yx(s2), 

...} 

1  yx  <-  x 

2  L  4-  {yx} 

3  while  dist(yx,  7/.)  ^  h  do 

4 

ki  <—  /rInterpolatedControl(a:*(  ■ , 

>A  xr,  yx) 

5 

k2  4—  LlnterpolatedControl(a:*(  ■ , 

),  h,  xr,  yx  +  ki) 

6 

yx  <-  yx  +  0.5(ki  +  k2) 

7 

L  4-  L  U  {yx} 
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3.5.2  Level  Sets 

The  level  sets  of  u  are  defined  to  be  {x  G  M2  |  -u(x)  =  C}  for  a  given  CgK.  Level  sets  can 
be  visualised  using  a  “contour”  function  included  in  most  mathematical  software  packages. 
The  figures  in  Section  4  were  generated  using  this  approach. 

A  coarse  approximation  to  the  C  level  set  of  u  can  be  obtained  by  monitoring  the  state 
of  the  AF  set  during  the  OUM  algorithm  [Sethian  &  Vladimirsky  2003,  see  Remark  7.4]. 
Let  umfn  =  min (i,j)eAFU(i,j)  and  U^x  =  rna x(  ij)£AF  U (i,  j),  where  AF  refers  to  the 
state  of  the  AF  set  at  the  nth  iteration  and  (i,j)  denotes  an  element  of  a  pair  of  adjacent 
mesh  co-ordinates  in  AF.  If  the  condition 

U7Z  uZx,  (29) 

is  satisfied  at  the  nth  iteration  of  the  OUM  algorithm,  then  the  corresponding  state  of  AF 
represents  a  coarse  approximation  to  the  C  level  set  of  u.  A  comparison  of  this  approach 
with  the  more  accurate  “contour”  function  approach  is  shown  in  Figure  9  in  Section  4. 


4  Examples 


The  primary  aim  of  this  section  is  to  employ  OrderedUpwindMethod  (Algorithm  8)  to  solve 
a  number  of  simple  path  planning  problems,  to  provide  figures  with  which  the  reader  can 
compare  output  from  their  implementation  of  OUM.  This  section  is  not  intended  to  be  a 
demonstration  of  the  utility  of  OUM  for  solving  real-world  problems.  Consequently  there 
will  only  be  a  very  limited  discussion  of  the  origin  of  the  running  costs  used  to  generate 
the  figures. 

In  addition  to  providing  the  reader  with  examples,  images  of  the  two  approaches  for 
visualising  level  sets  that  were  discussed  in  Section  3.5.2  are  overlaid  in  Figure  9  for  an 
anisotropic  example.  To  demonstrate  the  validity  of  OrderedUpwindMethod,  the  level  sets 
produced  by  OrderedUpwindMethod  for  another  anisotropic  running  cost  and  an  image  of 
the  corresponding  figure  from  Sethian  &  Vladimirsky  [2003]  are  overlaid  in  Figure  12. 

OrderedUpwindMethod  was  run  on  a  uniform  mesh  of  equilateral  triangles  on  the  unit 
square  with  h  =  0.00275  (3902  mesh  points)  to  generate  the  figures  in  this  section;  an 
exception  is  Figure  12,  which  was  generated  on  a  Cartesian  grid  with  spacing  5  =  0.0026 
( h  =  \f2b  with  3852  grid  points).20  In  all  figures  the  terminal  cost  is  given  by 


ff(x) 


0,  x  e  dT 
oo,  otherwise, 


where  dT  is  the  boundary  of  the  target  set;  this  ensures  that  all  optimal  paths  hit  the 
target  set.21  The  target  set  is  chosen  to  be  the  singleton  {x-j},  which  is  approximated  by 
the  neighbours  of  x-j. 

^Correspondence  with  A.  Vladimirsky  revealed  that  Figure  5  from  Sethian  &  Vladimirsky  [2003]  was 
generated  on  a  Cartesian  grid. 

21  Theoretically,  this  terminal  cost  is  required  to  be  mollified.  However,  in  practice,  the  value  function  is 
simply  set  to  zero  on  the  boundary  of  the  target  set. 


UNCLASSIFIED 


27 


DSTO-TR-2815 


UNCLASSIFIED 


Figure  5:  Uniform  running  cost  (R  =  1 ),  showing  level  sets  and  an  optimal  path  with 
initial  position  x  =  (0.1, 0.1)  and  target  location  xq-  =  (1, 1). 


4.1  Isotropic  Running  Costs 

The  first  isotropic  example  is  the  trivial  case  of  a  uniform  running  cost,  given  by  R(x)  =  1 
with  anisotropy  function  T(x)  =  1.  In  Figure  5  it  can  be  seen  that,  as  expected,  the 
value  function  is  simply  the  distance  to  the  boundary  of  the  target  set,  the  level  sets  are 
concentric  circles  centred  at  the  target,  and  the  optimal  paths  are  the  shortest  straight-line 
paths  from  given  initial  positions  to  the  boundary  of  the  target  set. 

The  second  isotropic  example  is  an  obstacle  avoidance  problem  taken  from  Kumar  & 
Vladimirsky  [2010],  and  is  displayed  in  Figures  6  and  7.  The  obstacles  are  three  squares 
with  side  length  Ls  =  0.1,  located  at  si  =  (0.25,0.5),  S2  =  (0.5, 0.3)  and  S3  =  (0.65,0.75). 
Let  the  shortest  distance  to  the  obstacles  be 

0(x)  =  min{||x  -  Si||oo,  ||x  -  s2||oo,  ||x  -  S3||oo}  . 

The  running  cost  is  given  by22 

[ _ - _ , 

Rfx)  =  <  1  +  0.8sin(47rxi)  sin(67TX2)  ’ 

[00, 

and  the  anisotropy  function  is  T(x)  =  1. 

Observe  in  Figures  5  and  6  that  the  optimal  paths  are  orthogonal  to  the  levels  sets  of 
the  value  function,  as  expected  for  isotropic  running  costs. 

22This  running  cost  should  be  mollified  to  be  consistent  with  the  theory  that  underpins  OUM.  Alter¬ 
natively,  the  solution  can  be  interpreted  in  terms  of  a  discontinuous  value  function  [Bardi  &  Capuzzo- 
Dolcetta  2008]. 


0(x )  >  0.5LS 
otherwise, 


(30) 
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Figure  6:  Isotropic  running  cost  (given  by  Equation  (30)),  showing  level  sets  and  an 
optimal  path  with  initial  position  x  =  (0.1,  0.1)  and  target  location  X7-  =  (1, 1).  The  black 
squares  represent  obstacles,  which  have  a  side  length  of  0.1  and  are  located  at  (0.25,0.5), 
(0.5, 0.3)  and  (0.65,0.75). 


Figure  7:  Isotropic  running  cost  (given  by  Equation  (30)),  showing  level  sets  and  a 
representation  of  a  subset  of  the  optimal  controls.  The  target  location  is  X7-  =  (1,1)  and 
the  {0.1,  0.2, . . . ,  1.2}  level  sets  are  displayed. 
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Figure  8:  Anisotropic  running  cost  (given  by  Equation  (31)),  showing  level  sets  and  an 
optimal  path  with  initial  position  x  =  (0.1, 0.1)  and  target  location  xj-  =  (0.5,  0.5). 


4.2  Anisotropic  Running  Costs 


The  first  anisotropic  example  originated  as  a  seismic  imaging  problem,  and  can  be  found 
in  Sethian  &  Vladimirsky  [2003]  and  Kumar  &  Vladimirsky  [2010].  The  running  cost  is 
given  by 


R(x,a)  =  1.25 


\ 


1  + 


15 


1  +  (0.497tcos(47txi))2 


(0.49vtcos(47txi),  —1)  •  a 


(31) 


and  the  anisotropy  function  is  Y(x)  =  4.  The  solution  to  this  seismic  imaging  problem  is 
visualised  in  Figure  8. 23 

Images  of  the  two  approaches  for  visualising  level  sets  that  were  discussed  in  Sec¬ 
tion  3.5.2  are  overlaid  in  Figure  9  for  this  anisotropic  example  (Equation  (31)).  Note  that 
raw  AF  data  (line  segments)  obtained  by  applying  the  condition  given  by  Equation  (29) 
is  shown  in  Figure  9,  together  with  the  level  sets  generated  by  a  “contour”  function.  The 
excellent  visual  agreement  that  can  be  observed  in  Figure  9  is  primarily  due  to  the  rel¬ 
atively  fine  mesh  used  to  produce  the  solution  data;  for  a  more  coarse  mesh,  differences 
between  these  approaches  are  more  noticeable. 

JiThere  are  small  qualitative  differences  between  Figure  8  and  the  corresponding  figure  in  Kumar  & 
Vladimirsky  [2010],  primarily  in  the  optimal  path.  Kumar  &  Vladimirsky  adapt  OUM  to  solve  constrained 
and  multiobjective  optimal  control  problems.  For  unconstrained  (and  single  objective)  optimal  control 
problems  their  numerical  method  coincides  with  OUM  in  the  limit  as  h  — >  0.  However,  for  a  given  mesh 
with  h  away  from  zero,  some  differences  in  the  solutions  produced  using  these  methods  is  to  be  expected. 
Furthermore,  the  corresponding  figure  in  Kumar  &  Vladimirsky  [2010]  was  generated  on  a  Cartesian  grid 
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Figure  9:  Anisotropic  running  cost  (given  by  Equation  (31)),  showing  level  sets  gen¬ 
erated  by  a  “ contour ”  function  (thick  dashed  curves)  and  the  level  sets  generated  using 
Equation  (29)  (thin  solid  curves).  The  {0.2,  0.4, . . .  ,2.0}  level  sets  are  displayed. 


The  final  anisotropic  example  is  related  to  the  geodesic  distance  on  a  manifold,  and 
comes  from  Sethian  &  Vladimirsky  [2003].  The  running  cost  is 

I?(x,  a)  =  yjl  +  (1.87T  (cos(27rxi)  sin(27TX2),  cos(27tx2)  sin(27rxi))  •  a.)2,  (32) 

and  the  anisotropy  function  is  Y(x)  =  5.75.  The  solution  to  this  example  is  visualised  in 
Figures  10  and  11. 

Observe  in  Figures  8  and  10  that  the  optimal  paths  are  not  necessarily  orthogonal  to 
the  levels  sets  of  the  value  function,  as  expected  for  anisotropic  running  costs. 

To  demonstrate  the  validity  of  OrderedUpwindMethod,  the  level  sets  produced  by 
Order edUpwindMethod  for  this  anisotropic  example  (Equation  (32))  and  an  image  of  the 
corresponding  figure  from  Sethian  &  Vladimirsky  [2003]  are  overlaid  in  Figure  12,  where 
excellent  visual  agreement  can  be  observed. 


with  2012  grid  points,  whereas  Figure  8  was  generated  on  a  uniform  mesh  of  equilateral  triangles  with  3902 
mesh  points. 
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Figure  10:  Anisotropic  running  cost  (given  by  Equation  (32)),  showing  level  sets  and  an 
optimal  path  with  initial  position  x  =  (—0.3,— 0.4)  and  target  location  x^-  =  (0,0).  The 
{0.05, 0.0973684, . . . ,  0.95}  level  sets  are  displayed. 


-0.4  -0.2  0.0  0.2  0.4 


Figure  11:  Anisotropic  running  cost  (given  by  Equation  (32)),  showing  level  sets  and  a 
representation  of  a  subset  of  the  optimal  controls.  The  target  location  is  xg-  =  (0, 0)  and 
the  {0.05,  0.0973684, . . . ,  0.95}  level  sets  are  displayed. 
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Figure  12:  Anisotropic  running  cost  (given  by  Equation  (32)),  showing  level  sets  gener¬ 
ated  by  OrderedUpwindMethod  (dashed  curves)  and  the  level  sets  reproduced  from  Figure  5 
of  Sethian  &  Vladimirsky  [2003]  (solid  curves);  both  sets  of  contours  were  generated  on  a 
3852  Cartesian  grid. 


5  Conclusion 

There  are  many  diverse  numerical  methods  that  can  be  applied  to  solving  path  planning 
problems,  however  most  of  these  are  either  not  valid  or  impractical  for  solving  anisotropic 
path  planning  problems.  Ordered  Upwind  Methods  (OUM)  are  a  family  of  numerical 
methods  for  approximating  the  viscosity  solution  of  static  Hamilton- Jacobi-Bellman  (HJB) 
equations,  and  have  been  tailored  to  solve  anisotropic  optimal  control  problems.  The  focus 
of  this  report  has  been  on  the  control-theoretic  OUM. 

There  is  little  information  in  the  literature  regarding  the  implementation  of  OUM, 
and  a  wide  range  of  computational  techniques  and  meticulous  algorithmic  considerations 
are  required  to  successfully  implement  OUM.  A  comprehensive,  generic  implementation  of 
OUM  has  been  documented  in  Section  3  of  this  report,  with  the  intention  of  minimising 
the  technical  barriers  to  employing  OUM  in  real-world  applications. 

The  OUM  algorithm  processes  subsets  of  the  computational  domain  to  compute  the 
numerical  value  function.  The  run-time  of  the  OUM  algorithm  can  be  significantly  reduced 
by  noting  that  elements  are  only  added  to  (or  removed  from)  the  AcceptedFront  from  a 
neighbourhood  of  the  most  recently  accepted  mesh  co-ordinate.  This  is  exploited  in  the 
AcceptedFront  update  rule  given  by  Equation  (24)  and  in  the  definition  of  the  AFUpdate 
function  (Algorithm  3). 

Note  that  the  major  contributors  to  the  run-time  of  the  OUM  algorithm  are  near 
front  construction  (refer  to  Algorithms  5  and  6)  and  the  evaluation  of  the  tentative  value 
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function  (Algorithm  7).  This  is  caused  by  the  recalculation  of  the  tentative  value  function 
to  account  for  the  changes  to  the  near  front  that  result  from  accepting  a  mesh  co-ordinate 
at  each  iteration  of  the  OUM  algorithm;  refer  to  step  7  of  Algorithm  8. 

If  the  anisotropy  function  satisfies  Y(x)  3>  l/li  on  a  significant  subset  of  the  do¬ 
main,  then  NF(x)  ~  AF  and  hence  the  run-time  of  OUM  may  considerably  increase. 
Decreasing  the  run-time  of  OUM  typically  requires  solving  the  HJB  equation  on  a  non- 
uniform  mesh.  One  such  approach  entails  refining  the  mesh  on  subsets  of  the  domain 
where  Y(x)  3>  1/h.  This  mesh  refinement  condition,  together  with  the  bisection  refine¬ 
ment  algorithm  of  Maubach  [1995]  and  the  data  structures  presented  in  Cline  &  Renka 
[1984],  have  been  implemented  by  the  author  and  the  initial  results  are  promising.  Other 
methods  require  two  passes  through  the  domain  to  construct  the  numerical  solution  of 
the  HJB  equation.  The  so-called  Patchy  Dynamic  Programming  scheme  of  Cacace  et  al. 
[2012]  requires  the  HJB  equation  to  be  solved  initially  on  a  coarse  mesh  to  determine  the 
subsets  of  the  domain,  known  as  patches,  where  the  solution  can  be  independently  com¬ 
puted  in  the  second  stage  of  the  scheme.  The  HJB  equation  is  then  solved  using  a  finer 
mesh  on  each  patch  utilizing  parallel  computing.  Alton  &  Mitchell  [2012]  have  developed 
a  variant  of  the  OUM  that  is  optimised  for  highly  non-uniform  meshes.  For  this  two-pass 
approach,  the  computational  stencil  is  first  computed  for  each  node  such  that  during  the 
second  pass,  the  HJB  equation  can  be  solved  using  a  fast  Dijkstra-like  method  where  the 
nodes  are  accepted  monotonically. 

The  aforementioned  techniques  for  decreasing  the  run-time  of  OUM  when  Y(x)  3>  1/h 
continue  to  be  investigated  by  the  author.  Applying  OUM  to  the  path  planning  of  military 
aircraft  traversing  hostile  environments  will  also  be  studied  in  future  work. 
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Appendix  A  Triangulated  Mesh  Based  on  a 

Cartesian  Grid 

An  adaption  of  the  key  mesh  dependent  functions  for  the  case  of  a  triangulated  mesh 
based  on  a  Cartesian  grid  is  presented  in  this  appendix. 

The  mesh  point  x(z,  j)  E  flh  indexed  by  the  mesh  co-ordinate  (i,j)  E  fi?  is  given  by 

x(i,  j)  =  xr  +  S{i,j) ,  (Al) 

where  5  is  the  grid  spacing  and  the  triangulation  diameter  h  =  y/28.  The  functions  for 
mesh  transformations  and  mesh  construction  follow  directly  from  Equation  (Al). 

There  are  many  ways  of  constructing  a  triangulated  mesh  from  a  Cartesian  grid,  and 
a  general  discussion  of  this  topic  is  beyond  the  scope  of  this  report.  Instead,  the  set 
of  neighbours  N(i,j)  which  has  the  same  mesh  co-ordinates  as  for  a  uniform  mesh  of 
equilateral  triangles  (see  Equation  (20))  is  shown  in  Figure  Al. 

The  modifications  of  the  FindSimplex  and  InterpolationCoef  f  icients  functions  for 
the  case  of  a  Cartesian  grid  can  be  found  in  Algorithms  13  and  14,  respectively. 


x(*  -  1,  j  +  1)  • 

•  x(i,j  +  1) 

x(*  -  1  ,j)  • 

•  0  x(l  +  1  ,j) 

*(*,  j  ~  1)  •  •  x(i+  1  ,j  -  1) 

Figure  Al:  The  N(i,j)  set  shown  mapped  onto  Qh  as  large  dots  for  the  case  of  a  tri¬ 
angulation  based  on  a  Cartesian  grid.  The  x(  • ,  •)  are  given  by  Equation  (Al)  and  the 
triangulation  diameter  h  =  y/25,  where  5  is  the  grid  spacing. 
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Algorithm  13:  FindSimplex  for  the  case  of  a  Cartesian  grid 


Input:  5,  x^,  and  xell  where  x  =  (xi,x2) 

Output:  {u,  v,w}  C  ff?,  where  uvw  is  a  simplex  with  adjacent  vertices  that 
contains  x 


1 

2 

3 

4 

5 

6 

7 

8 
9 


{xc,  xi, X2, X3, X4, X5, Xg}  <—  MeshPoint (5, xj,  ^(Nearest Index(<5,  x-j,  x))) 
if  x2  >  yc  then 
if  x\  >  xc  then 
J  return  {xc, xi, X2} 
else 

if  (x  —  xc)  •  (X4  —  xc)  >  x/ 0.5(5||x  —  xc||  then 
j  return  {xc,  X3,  X4} 
else 

[_  return  {xc,x2,x3} 


10 

11 

12 

13 

14 

15 

16 
17 


else 

if  x\  <  xc  then 
j  return  {xc,  X4,  X5} 
else 

if  (x  —  xc)  •  (xi  —  xc)  >  x/ 0.5(5 1 1 x  —  xc ||  then 
j  return  {xc,  xq,  xi} 
else 

j_  return  {xc,x5,x6} 


Algorithm  14:  InterpolationCoef  f  icients  for  the  case  of  a  Cartesian  grid 

Input:  i,  x  G  fl,  and  {u,  v,w}  C  D?,  where  uvw  is  a  simplex  with  adjacent 
vertices  that  contains  x 

Output:  {Ci,  C2;  C3}  U  M3,  which  are  barycentric  co-ordinates  of  x  with  respect  to 
{u,  V,  w}  such  that  Cl  +  C2  +  C3  =  1 

1  C  ^  1/52 

2  z  X  —  w 

3  Cl  <—  Cz  ■  (u  —  w) 

4  C*2  <—  Cz  ■  (v  —  w) 

5  {Ci!  C2,  C3}  {Ci,  c2, 1  -  Ci  -  c2j 
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Appendix  B  Verification  of  the  AcceptedFront 

update  rule 

The  aim  of  this  appendix  is  to  verify  the  AcceptedFront  update  rule  given  by  Equation  (24). 

To  begin,  let  the  states  of  the  Accepted  and  Considered  sets  that  are  used  to  update 
the  AcceptedFront  during  the  nth  iteration  of  the  OUM  algorithm  be  given  by  An  and 
Cn,  respectively.  Then  Equation  (24)  can  be  expressed  as 

AcceptedFrontn  =  ( AcceptedFrontn_1  \  lV(xn))  U  Adj acentTo(An  fi  N(xn),  Cn),  (Bl) 

where  xn  is  the  most  recently  accepted  mesh  point  at  the  nth  iteration.24  Recall  that,  by 
definition, 

AcceptedFrontn  =  AdjacentTo(An,  Cn).  (B2) 

The  remainder  of  this  appendix  is  devoted  to  proving  that  Equations  (Bl)  and  (B2) 
generate  the  same  sets. 

For  any  sets  X  and  Y,  X  =  ( X  \F)U(InF).  Therefore 

AcceptedFrontn  =  ( AcceptedFrontn  \  IV  (xn))  U  AdjacentTo(A„  n  lV(xn),  Cn), 
by  Equation  (B2)  and  the  definition  of  AdjacentTo  in  Equation  (22).  Consequently,  if 
AcceptedFrontn  \  7V(xn)  =  AcceptedFrontn_1  \  lV(xn), 
then  Equations  (Bl)  and  (B2)  will  generate  the  same  sets.  Observe  that25 
AcceptedFrontn  \  lV(xn)  =  An  Cl  N{Cn)  Cl  lV(xn)c 

=  (An-i  u  {xn})  n  N{Cn)  n  ]v(xn)c 
=  An_!  n  N(cn)  n  F(xn)c, 

as  An  =  An- 1  U  {xn}  and  {xn}  0  N(5cn)c  =  0.  Furthermore, 

AcceptedFrontn_1  \  N(ptn)  =  An- \  0  N(Cn- i)  Cl  Al(xn)c. 

Therefore  if 

An-1  n  N(Cn )  n  N(xn)c  =  An _!  n  N(Cn-i)  n  N(xn)c,  (B3) 

then  Equations  (Bl)  and  (B2)  will  generate  the  same  sets. 

To  establish  Equation  (B3),  the  following  identity  is  required  for  sets  X,  Y  C 

N(XUY)  =  (N(X)  U  lV(y))\  Adj  acentTo(X,y),  if  X  0  Y  =  0.  (B4) 

Let  XHY  =  0.  If  X,  Y  also  satisfy  AdjacentTo(X,  Y)  =  0,  then  N(XUY)  =  N(X)UN(Y). 
However  if  AdjacentTo(X,  Y)  ^  0,  then  N(X)  will  contain  elements  that  belong  to  Y , 

24Here  it  is  more  convenient  to  work  with  mesh  points  instead  of  mesh  co-ordinates,  which  are  used  in 
Equation  (24). 

25Recall  that  X  \  Y  =  X  fl  Yc,  where  Yc  is  the  complement  of  Y. 
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and  N(Y)  will  contain  elements  of  X.  Equation  (B4)  follows  from  these  observations, 
noting  that  AdjacentTo(X,  Y)  =  AdjacentTo(Y,  X )  provided  X  D  Y  =  0. 

The  Considered  set  at  the  nth  iteration  of  the  OUM  algorithm  can  be  related  to  Cn- 1 
via 

Cn  U  {X„}  =  Cn-X  U  C™W, 

where  CZW  C  Cn  is  the  set  of  new  points  added  to  Considered  during  the  previous 
iteration.  Consequently 

N(Cn  U  {xn})  =  IY(Cn_r  U  CZW )■  (B5) 


Continuing,26 

N(Cn  U  {xn})  n  N(5tn)c  =  ( N(Cn )  U  N(xn))  n  (Cn  n  N(5cn))c  n  IV(xn)c 

=  (N(cn)  u  JV(xn))  n  iv(xn)c 
=  at(C'„)  nF(xn)c, 

by  Equation  (B4),  noting  that  Cn  Cl  IV(xn)  C  IV(xn)  and  7V(xn)  n  IV(xra)c  =  0.  Therefore 
An-i  n  iv(<cn)  n  N(xre)c  =  An_!  n  A^(Cn  u  {xn})  n  iv(xn)c.  (B6) 

Similarly,27 

An_!  n  N(Cn.  1  u  CT")  =  An_!  n  (iv(cn_i)  u  iv^r"))  n  (Cn-i  n  N(CeM'))c 

=  An_ !  n  u  iv(c,rT)  n  (iv(C7n_i)  u  iv(cr™)) 

=  ((An_!  n  c^)  u  (4_!  n  N(CZUT))  n  (iv(cn_i)  u  iv(C7™)) 
=  An_!  n  (iv^-i)  u  ivcgT'’)) 

=  (An_!  n  N(cn-i))  u  (An_!  n  N(CZW )) 

=  An_imv(cn_i), 

using  Equation  (B4),  noting  that  C^_1  =  An_ i  U  Farn- 1,  An_i  C  An_i  Cl  N(C'”e“’)c,  and 
An_i  Cl  N(Cn£W)  =  AdjacentTo(An_i,  CZW)  =  0,  as  C™6™  C  Farn- 1-  It  follows  that 

An_r  n  A^(cn_!)  n  iv(xn)c  =  A„_!  n  iv(cn_i  u  czw)  n  iv(xn)c.  (B7) 

Combining  Equations  (B5)  to  (B7)  yields 

An- 1  n  N(cn)  n  N(5cn)c  =  An- 1  n  iv(cn  u  {xn})  n  iv(xn)c 

=  An-i  n  iv(cn_i  u  czw)  n  iv(xn)c 

=  A,_1niv(C,n_1)nF(xn)c. 

Therefore  Equation  (B3)  is  verified,  and  hence  Equations  (Bl)  and  (B2)  generate  the  same 
sets. 

26 Recall  that  (. X  U  Y)  n  Z  =  {X  n  Z)  U  (Y  n  Z),  and  (X  n  Y)  U  Z  =  (X  U  Z)  D  (Y  U  Z). 

27Dc  Morgan’s  Laws  state  that  (X  U  Y)c  =  Xc  n  Yc  and  (. X  n  Y)c  =  Xc  U  Yc. 
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